function conf = spm_cfg_deformations
% Configuration file for deformation jobs
%_______________________________________________________________________
% Copyright (C) 2008-2016 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_cfg_deformations.m 7700 2019-11-21 17:09:15Z john $

hsummary = {
'Utility for working with deformation fields.',...
['They can be loaded, inverted, combined etc, and the results ',...
'either saved to disk, or applied to some image or surface file. ',...
'This utility was intended for imaging experts and may therefore ',...
'be a bit difficult for naive users. ',...
'It provides a great deal of flexibility, which may be confusing to some.']};

hinv = {[...
'Creates the inverse of a deformation field. ',...
'Deformations are assumed to be one-to-one, in which case they ',...
'have a unique inverse.  If y'':A->B is the inverse of y:B->A, then ',...
'y'' o y = y o y'' = Id, where Id is the identity transform.'],...
'',...
'Deformations are inverted using the method described in the appendix of:',...
['    * Ashburner J, Andersson JLR & Friston KJ (2000) ',...
 '"Image Registration using a Symmetric Prior - in Three-Dimensions." ',...
 'Human Brain Mapping 9(4):212-225']};

hcomp = {[...
'Deformation fields can be thought of as mappings. ',...
'These can be combined by the operation of "composition", which is ',...
'usually denoted by a circle "o". ',...
'Suppose x:A->B and y:B->C are two mappings, where A, B and C refer ',...
'to domains in 3 dimensions. ',...
'Each element a in A points to element x(a) in B. ',...
'This in turn points to element y(x(a)) in C, so we have a mapping ',...
'from A to C. ',...
'The composition of these mappings is denoted by yox:A->C. ',...
'Compositions can be combined in an associative way, such that zo(yox) = (zoy)ox.'],...
'',[...
'In this utility, the right-to-left order of the compositions is ',...
'from top to bottom (note that the rightmost deformation would ',...
'actually be applied first).']};

hsn = {[...
'Spatial normalisation, and the unified segmentation model of ',...
'SPM5 save a parameterisation of deformation fields.  These consist ',...
'of a combination of an affine transform, and nonlinear warps that ',...
'are parameterised by a linear combination of cosine transform ',...
'basis functions.  These are saved in *_sn.mat files, which can be ',...
'converted to deformation fields.']};

hvox = {[...
'Specify the voxel sizes of the deformation field to be produced. ',...
'Non-finite values will default to the voxel sizes of the template image',...
'that was originally used to estimate the deformation.']};

hbb = {[...
'Specify the bounding box of the deformation field to be produced. ',...
'Non-finite values will default to the bounding box of the template image',...
'that was originally used to estimate the deformation.']};

himgr = {[...
'Deformations can be thought of as vector fields, and represented ',...
'by three-volume images.  In SPM, deformation fields are saved in ',...
'NIfTI format, with dimensions xdim x ydim x zdim x 1 x 3. ',...
'Each voxel contains the x, y and z mm coordinates of where the deformation points.']};

himgw = {[...
'Save the result as a three-volume image.  "y_" will be prepended to the ',...
'filename.']};

hdetw = {[...
'Save the Jacobian determinants as an image.  "j_" will be prepended to the ',...
'filename.']};

happly = {[...
'Apply the resulting deformation field to some images. ',...
'The filenames will be prepended by "w".']};

hmatname = {...
'Specify the _sn.mat to be used.'};

himg = {...
'Specify the image file on which to base the dimensions, orientation etc.'};

hid = {[...
'This option generates an identity transform, but this can be useful for ',...
'changing the dimensions of the resulting deformation (and any images that ',...
'are generated from it).  Dimensions, orientation etc are derived from ',...
'an image.']};

hidbbvox = {[...
'This option generates an identity transform, but this can be useful for ',...
'changing the dimensions of the resulting deformation (and any images that ',...
'are generated from it).  Dimensions, orientation etc are derived from ',...
'a specified bounding box and voxel dimensions.']};

def          = files('Deformation Field','def','.*y_.*\.nii$',[1 1]);
def.help     = himgr;

matname      = files('Parameter File','matname','.*_sn\.mat$',[1 1]);
matname.help = hmatname;

vox          = entry('Voxel sizes','vox','r',[1 3]);
vox.val      = {[NaN NaN NaN]};
vox.help     = hvox;

bb           = entry('Bounding box','bb','r',[2 3]);
bb.val       = {[NaN NaN NaN;NaN NaN NaN]};
bb.help      = hbb;

sn2def       = branch('Imported _sn.mat','sn2def',{matname,vox,bb});
sn2def.help  = hsn;

img          = files('Image to base Id on','space','nifti',[1 1]);
img.help     = himg;

id           = branch('Identity (Reference Image)','id',{img});
id.help      = hid;

idbbvox      = branch('Identity (Bounding Box and Voxel Size)','idbbvox',{vox, bb});
idbbvox.help = hidbbvox;

ffield = files('Flow field','flowfield','nifti',[1 1]);
ffield.ufilter = '^u_.*';
ffield.help = {...
    ['The flow field stores the deformation information. '...
     'The same field can be used for both forward or backward deformations '...
     '(or even, in principle, half way or exaggerated deformations).']};
%------------------------------------------------------------------------
forbak = mnu('Forward/Backwards','times',{'Backward','Forward'},{[1 0],[0 1]});
forbak.val  = {[1 0]};
forbak.help = {[...
    'The direction of the Dartel flow.  '...
    'Note that a backward transform will warp an individual subject''s '...
    'to match the template (ie maps from template to individual). '...
    'A forward transform will warp the template image to the individual.']};
%------------------------------------------------------------------------
K = mnu('Time Steps','K',...
        {'1','2','4','8','16','32','64','128','256','512'},...
        {0,1,2,3,4,5,6,7,8,9});
K.val  = {6};
K.help = {...
    ['The number of time points used for solving the '...
     'partial differential equations.  A single time point would be '...
     'equivalent to a small deformation model. '...
     'Smaller values allow faster computations, '...
     'but are less accurate in terms '...
     'of inverse consistency and may result in the one-to-one mapping '...
     'breaking down.']};
% ---------------------------------------------------------------------
template        = cfg_files;
template.tag    = 'template';
template.name   = 'Dartel Template';
template.filter = 'nifti';
template.num    = [0 1];
template.val    = {{''}};
template.help   = {...
['Select the final Template file generated by Dartel. This will be affine '...
 'registered with a TPM file, such that the resulting spatially normalised '...
 'images are closer aligned to MNI space. Leave empty if you do not wish to '...
 'incorporate a transform to MNI space '...
 '(ie just click ``done'' on the file selector, without selecting any images).']};
%------------------------------------------------------------------------
drtl = branch('Dartel flow','dartel',{ffield,forbak,K,template});
drtl.help = {'Imported Dartel flow field.'};
%------------------------------------------------------------------------
other = {drtl,def,id,idbbvox,sn2def};

img          = files('Image to base inverse on','space','nifti',[1 1]);
img.help     = himg;

comp0        = repeat('Composition','comp',other);
comp0.help   = hcomp;

iv0          = branch('Inverse','inv',{comp0,img});
iv0.help     = hinv;

comp1        = repeat('Composition','comp',[other,{iv0},{comp0}]);
comp1.num    = [1 Inf];
comp1.help   = hcomp;

iv1          = branch('Inverse','inv',{comp1,img});
iv1.help     = hinv;

comp2        = repeat('Composition','comp',[other,{iv1},{comp1}]);
comp2.num    = [1 Inf];
comp2.help   = hcomp;

iv2          = branch('Inverse','inv',{comp2,img});
iv2.help     = hinv;

comp         = repeat('Composition','comp',[other,{iv2},{comp2}]);
comp.num     = [1 Inf];
comp.help    = hcomp;

saveas       = entry('Save as','ofname','s',[0 Inf]);
saveas.help  = himgw;

savedas       = entry('Save as','ofname','s',[0 Inf]);
savedas.help  = hdetw;

applyto      = files('Apply to','fnames','nifti',[0 Inf]);
applyto.help = happly;

savepwd      = cfg_const;
savepwd.name = 'Current directory';
savepwd.tag  = 'savepwd';
savepwd.val  = {1};
savepwd.help = {['All created files (deformation fields and warped images) ' ...
                 'are written to the current directory.']};

savesrc      = cfg_const;
savesrc.name = 'Source directories';
savesrc.tag  = 'savesrc';
savesrc.val  = {1};
savesrc.help = {['The combined deformation field is written into the ' ...
                 'directory of the first deformation field, warped images ' ...
                 'are written to the same directories as the source ' ...
                 'images.']};

savedef      = cfg_const;
savedef.name = 'Source directory (deformation)';
savedef.tag  = 'savedef';
savedef.val  = {1};
savedef.help = {['The combined deformation field and the warped images ' ...
                 'are written into the directory of the first deformation ' ...
                 'field.']};

saveusr      = files('Output directory','saveusr','dir',[1 1]);
saveusr.help = {['The combined deformation field and the warped images ' ...
                 'are written into the specified directory.']};

savedir      = cfg_choice;
savedir.name = 'Output destination';
savedir.tag  = 'savedir';
savedir.values = {savepwd savesrc saveusr};
savedir.val  = {savepwd};

savedir1      = cfg_choice;
savedir1.name = 'Output destination';
savedir1.tag  = 'savedir';
savedir1.values = {savepwd saveusr};
savedir1.val  = {savepwd};

interp      = cfg_menu;
interp.name = 'Interpolation';
interp.tag  = 'interp';
interp.labels = {'Nearest neighbour','Trilinear','2nd Degree B-spline',...
'3rd Degree B-Spline ','4th Degree B-Spline ','5th Degree B-Spline',...
'6th Degree B-Spline','7th Degree B-Spline','Categorical'};
interp.values = {0,1,2,3,4,5,6,7,-1};
interp.def  = @(val)spm_get_defaults('normalise.write.interp',val{:});
interp.help    = {
                  ['The method by which the images are sampled when ' ...
                  'being written in a different space. ' ...
                  '(Note that Inf or NaN values are treated as zero, ' ...
                  'rather than as missing data)']
                  '    Nearest Neighbour:'
                  '      - Fastest, but not normally recommended.'
                  '    Trilinear Interpolation:'
                  '      - OK for PET, realigned fMRI, or segmentations'
                  '    B-spline Interpolation:'
                  ['      - Better quality (but slower) interpolation' ...
                  '/* \cite{thevenaz00a}*/, especially with higher ' ...
                  'degree splines. Can produce values outside the ' ...
                  'original range (e.g. small negative values from an ' ...
                  'originally all positive image).']
                  '    Categorical:'
                  ['       - Slow (particularly when there are lots of '...
                  'categories). This is intended to warp categorical images ' ...
                  'such as label maps.']
}';

% ---------------------------------------------------------------------
fwhm         = cfg_entry;
fwhm.tag     = 'fwhm';
fwhm.name    = 'Gaussian FWHM';
fwhm.val     = {[0 0 0]};
fwhm.strtype = 'r';
fwhm.num     = [1 3];
fwhm.help    = {'Specify the full-width at half maximum (FWHM) of the Gaussian blurring kernel in mm. Three values should be entered, denoting the FWHM in the x, y and z directions.'};
% ---------------------------------------------------------------------

% ---------------------------------------------------------------------
prefix         = cfg_entry;
prefix.tag     = 'prefix';
prefix.name    = 'Filename Prefix';
prefix.val     = {''};
prefix.strtype = 's';
prefix.num     = [0 Inf];
prefix.help    = {'The name of the output file(s) will be the name of the input file(s) prefixed with this prefix. Leave empty to use SPM default prefixes.'};
% ---------------------------------------------------------------------

% ---------------------------------------------------------------------
mask         = cfg_menu;
mask.tag     = 'mask';
mask.name    = 'Masking';
mask.help    = {'Because of subject motion, different images are likely to have different patterns of zeros from where it was not possible to sample data. With masking enabled, the program searches through the whole time series looking for voxels which need to be sampled from outside the original images. Where this occurs, that voxel is set to zero for the whole set of images (unless the image format can represent NaN, in which case NaNs are used where possible).'};
mask.labels = {
               'Mask images'
               'Dont mask images'
}';
mask.values = {1 0};
mask.val    = {1};
% ---------------------------------------------------------------------

% ---------------------------------------------------------------------
preserve         = cfg_menu;
preserve.tag     = 'preserve';
preserve.name    = 'Preserve';
preserve.help    = {
'Preserve Concentrations: Smoothed spatially normalised images (sw*) represent weighted averages of the signal under the smoothing kernel, approximately preserving the intensities of the original images. This option is currently suggested for eg fMRI.'
''
'Preserve Amount: Smoothed and spatially normalised images preserve the total amount of signal from each region in the images (smw*). Areas that are expanded during warping are correspondingly reduced in intensity. This option is suggested for VBM.'
''
'Preserve Labels: This is intended for warping label images. While it is quite slow to run, it is intended to give more accurately warped categorical data.'
}';
preserve.labels = {
                   'Preserve Concentrations (no "modulation")'
                   'Preserve Amount ("modulation")'
                   'Preserve Labels (categorical data)'
}';
preserve.values = {0 1 2};
preserve.val    = {0};
% ---------------------------------------------------------------------

% ---------------------------------------------------------------------
fromimage       = cfg_files;
fromimage.name   = 'Image Defined';
fromimage.tag    = 'file';
fromimage.filter = 'nifti';
fromimage.num    = [1 1];
fromimage.help   = {'Use the dimensions, orientation etc of some pre-existing image.'};
% ---------------------------------------------------------------------

% ---------------------------------------------------------------------
surfa        = cfg_files;
surfa.name   = 'Surface';
surfa.tag    = 'surface';
surfa.filter = 'mesh';
surfa.num    = [1 Inf];
surfa.help   = {'Select a GIFTI file to warp.'};
% ---------------------------------------------------------------------

bbvox         = cfg_branch;
bbvox.name    = 'User Defined';
bbvox.tag     = 'bbvox';
bbvox.val     = {bb,vox};
bbvox.help   = {[...
'The part of the deformation to use is specified by defining the bounding box and ',...
'voxel sizes that you would like to have. This is probably stating the obvious to many ',...
'but smaller voxels and a broader bounding box will take up more disk space, but may ',...
'give a little more accuracy.']};

deffov        = cfg_choice;
deffov.name   = 'Field of View';
deffov.tag    = 'fov';
deffov.values = {fromimage,bbvox};
deffov.help   = {[...
'The dimensions and voxel size of the resulting deformation may be defined from some image, ',...
'or by specifying voxel sizes and a bounding box.']};

savedef       = cfg_branch;
savedef.name  = 'Save Deformation';
savedef.tag   = 'savedef';
savedef.val   ={saveas,savedir1};
savedef.help  = {'The deformation may be saved to disk as a ``y_*.nii'''' file.'};

savedet       = cfg_branch;
savedet.name  = 'Save Jacobian Determinants';
savedet.tag   = 'savejac';
savedet.val   ={savedas,savedir1};
savedet.help  = {'The Jacobian determinants may be saved to disk as a ``j_*.nii'''' file.'};

pullback      = cfg_branch;
pullback.name = 'Pullback';
pullback.tag  = 'pull';
pullback.val  = {applyto,savedir,interp,mask,fwhm,prefix};
pullback.help = {[...
'This is the old way of warping images, which involves resampling images based on a mapping from ',...
'the new (warped) image space back to the original image.  ',...
'The deformation should be the inverse of the deformation that would be used for the pushforward procedure.']};

weight        = cfg_files;
weight.name   = 'Weight Image';
weight.tag    = 'weight';
weight.filter = 'nifti';
weight.num    = [0 1];
weight.help   = {'Select an image file to weight the warped data with.  This is optional, but the idea is the same as was used by JE Lee et al (2009) in their ``A study of diffusion tensor imaging by tissue-specific, smoothing-compensated voxel-based analysis'''' paper.  In principle, a mask of (eg) white matter could be supplied, such that the warped images contain average signal intensities in WM.'};
weight.val    = {{''}};

% add note on aliasing to fwhm.help for Pushforward
fwhm.help     = {[fwhm.help{1} ' Note that you can specify [0 0 0], ',...
    'but any "modulated" data will show aliasing, which occurs because of ',...
    'the way the warped images are generated.']};

pushfo        = cfg_branch;
pushfo.name   = 'Pushforward';
pushfo.tag    = 'push';
pushfo.val    = {applyto,weight,savedir,deffov,preserve,fwhm,prefix};
pushfo.help   = {[...
'This is a newer way of warping images (for SPM at least), and involves the ',...
'forward pushing of voxel values from the original image into the appropriate place in the warped image. ',...
'The deformation field should be the inverse of the one used for the pullback procedure.'],...
'',...
[...
'``Smoothed'''' (blurred) spatially normalised images are generated in such a ',...
'way that the original signal is preserved. Normalised images are ',...
'generated by a ``pushing'''' rather than a ``pulling'''' (the usual) procedure. ',...
'Note that a procedure related to trilinear interpolation is used, and no masking is done.  It ',...
'is therefore recommended that the images are realigned and resliced ',...
'before they are spatially normalised, in order to benefit from motion correction using higher order interpolation.  Alternatively, contrast images ',...
'generated from unsmoothed native-space fMRI/PET data can be spatially ',...
'normalised for a 2nd level analysis.'],[...
'Two ``preserve'''' options are provided.  One of them should do the ',...
'equavalent of generating smoothed ``modulated'''' spatially normalised ',...
'images.  The other does the equivalent of smoothing the modulated ',...
'normalised fMRI/PET, and dividing by the smoothed Jacobian determinants.']};

pushsurf      = cfg_branch;
pushsurf.name = 'Surface';
pushsurf.tag  = 'surf';
pushsurf.val  = {surfa,savedir};
pushsurf.help = {[...
'Surfaces may be warped using the resulting deformation. ',...
'Note that a procedure similar to the pushforward is used, so the deformation should ',...
'be the inverse of the one that would be used for spatially normalising images via the pullback procedure.']};

output        = cfg_repeat;
output.name   = 'Output';
output.tag    = 'out';
output.values = {savedef,pullback, pushfo,pushsurf,savedet};
output.help = {[...
'Various output options are available.  ',...
'The deformation may be saved to disk as a ``y_*.nii'''' file.',...
'Images may be warped using the resulting deformation, either using a ``pullback'''' procedure, or a ``pushforward''''.',...
'The old style of spatial normalisation involved the pullback, whereas the pushforward requires ',...
'the inverse of the deformation used by the pullback.  ',...
'Finally, the deformation may be used to warp a GIFTI surface file.']};

conf         = exbranch('Deformations','defs',{comp,output});
conf.prog    = @spm_deformations;
conf.vout    = @vout;
conf.help    = hsummary;

%==========================================================================
function vo = vout(job)
vo = [];
savedef   = false;
saveimage = false;
savesurf  = false;
savejac   = false;
for i=1:numel(job.out)
    out = job.out{i};
    if isfield(out,'savedef') && ~savedef
        savedef = true;
        if isempty(vo), vo = cfg_dep; else vo(end+1) = cfg_dep; end
        vo(end).sname      = 'Deformation';
        vo(end).src_output = substruct('.','def');
        vo(end).tgt_spec   = cfg_findspec({{'filter','nifti'}});
    end
    if (isfield(out,'pull') || isfield(out,'push')) && ~saveimage
        saveimage = true;
        if isempty(vo), vo = cfg_dep; else vo(end+1) = cfg_dep; end
        vo(end).sname      = 'Warped Images';
        vo(end).src_output = substruct('.','warped');
        vo(end).tgt_spec   = cfg_findspec({{'filter','image'}});
    end
    if isfield(out,'surf') && ~savesurf
        savesurf = true;
        if isempty(vo), vo = cfg_dep; else vo(end+1) = cfg_dep; end
        vo(end).sname      = 'Warped Surfaces';
        vo(end).src_output = substruct('.','surf');
        vo(end).tgt_spec   = cfg_findspec({{'filter','mesh'}});
    end
    if isfield(out,'savejac') && ~savejac
        savejac = true;
        if isempty(vo), vo = cfg_dep; else vo(end+1) = cfg_dep; end
        vo(end).sname      = 'Jacobian';
        vo(end).src_output = substruct('.','jac');
        vo(end).tgt_spec   = cfg_findspec({{'filter','image'}});
    end
end

%==========================================================================
function entry_item = entry(name, tag, strtype, num)
entry_item         = cfg_entry;
entry_item.name    = name;
entry_item.tag     = tag;
entry_item.strtype = strtype;
entry_item.num     = num;

function files_item = files(name, tag, fltr, num)
files_item        = cfg_files;
files_item.name   = name;
files_item.tag    = tag;
files_item.filter = fltr;
files_item.num    = num;

function branch_item = branch(name, tag, val)
branch_item      = cfg_branch;
branch_item.name = name;
branch_item.tag  = tag;
branch_item.val  = val;

function exbranch_item = exbranch(name, tag, val)
exbranch_item      = cfg_exbranch;
exbranch_item.name = name;
exbranch_item.tag  = tag;
exbranch_item.val  = val;

function repeat_item = repeat(name, tag, values)
repeat_item        = cfg_repeat;
repeat_item.name   = name;
repeat_item.tag    = tag;
repeat_item.values = values;

function menu_item = mnu(name, tag, labels, values)
menu_item        = cfg_menu;
menu_item.name   = name;
menu_item.tag    = tag;
menu_item.labels = labels;
menu_item.values = values;


function [ n, x, indx ] = i4vec_index_insert ( n, x, indx, xval )

%*****************************************************************************80
%
%% I4VEC_INDEX_INSERT inserts a value in an indexed sorted I4VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the size of the current list.
%
%    Input, integer X(N), the list.
%
%    Input, integer INDX(N), the sort index of the list.
%
%    Input, integer XVAL, the value to be sought.
%
%    Output, integer N, the size of the current list.
%
%    Output, integer X(N), the list.
%
%    Output, integer INDX(N), the sort index of the list.
%
  if ( n <= 0 )
    n = 1;
    x = [ xval ];
    indx = [ 1 ];
    return
  end

  [ less, equal, more ] = i4vec_index_search ( n, x, indx, xval );

  x(n+1) = xval;
  indx(n+1:-1:more+1) = indx(n:-1:more);
  indx(more) = n + 1;
  n = n + 1;

  return
end


function mask = compute_conv2_valid_mask(imsz, fsz)

% compute a mask of size IMSZ with 1 entry at valid pixel positions and 0
% invalid oens
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date:  $
%   $Revision: $

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

mask = ones(imsz);

h1   = floor(fsz(1)/2);
if fsz(1) == 1
    ;
elseif fsz(1) == 2
    mask(end,:) = 0;
elseif mod(fsz(1), 2) == 0
    mask(1:h1-1, :)         = 0;
    mask(end-h1+1:end, :)   = 0;
else
    mask(1:h1, :)    = 0;
    mask(end-h1+1:end, :)   = 0;
end;

h2   = floor(fsz(2)/2);

if fsz(2) == 1
    ;
elseif fsz(2) == 2
    mask(:, end) = 0;
elseif mod(fsz(2), 2) == 0
    mask(:, 1:h2-1)    = 0;
    mask(:, end-h2+1:end)   = 0;
else
    mask(:, 1:h2)    = 0;
    mask(:, end-h2+1:end)   = 0;
end;


% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
% Copyright 2015-2018 Jonas Koennemanm, Giovanni Licitra
% Redistribution is permitted under the 3-Clause BSD License terms. Please
% ensure the above copyright notice is visible in any derived work.
%
function p_vec = pendulum_sim

  conf = struct;
  conf.l = 1;
  conf.m = 1;

  simulator = ocl.Simulator(@ocl.examples.pendulum.varsfun, ...
    @ocl.examples.pendulum.daefun, ...
    @ocl.examples.pendulum.icfun, ...
    'userdata', conf);

  x0 = simulator.getStates();
  x0.p.set([0;conf.l]);
  x0.v.set([-0.5;-1]);

  times = 0:0.1:4;

  figure
  simulator.reset(x0);

  p_vec = zeros(2,length(times));
  p_vec(:,1) = x0.p.value;

  for k=1:length(times)-1

    dt = times(k+1)-times(k);
    [x,~] = simulator.step(10, dt);

    p_vec(:,k+1) = x.p.value;
  end

  ocl.examples.pendulum.animate(conf.l, p_vec, times);
  snapnow;
end


function [EE,I] = conservative_edge_matching(E_orig,varargin)
  % CONSERVATIVE_EDGE_MATCHING  Find set of given edges so that no two edges
  % share a vertex.
  %
  % [EE,I] = conservative_edge_matching(E)
  %
  % Inputs:
  %   E  #E by 2 list of edges
  %   Optional:
  %      'Method'  followed by one of the following:
  %        'one-per-component'  simply take one edge per conneceted component
  %          of edges
  %        'random'  use a randomized algorithm to try to grab many per
  %          component
  % Outputs:
  %   EE  #EE by 2 list of edges
  %   I  #EE list of indices into E, so that EE = E(I,:)
  %

  if size(E_orig,1) == 0
    EE = E_orig;
    I = [];
    C = [];
    return;
  end

  % default values
  method = 'one-per-component';
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Method'}, ...
    {'method'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  % Do this in reduced graph of just vertices incident on E. Otherwise this is
  % O(max(E(:))) rather than O(size(E))
  [~,~,E] = unique(E_orig(:));
  E = reshape(E,size(E_orig));

  switch method
  case 'one-per-component'
    % Find one edge per connected component of edges (ideally we would find a
    % "perfect matching")
    ne = size(E,1);
    C = connected_components(E);
    C = C(E(:,1));
    % Use reverse order so we can take max
    E2C = sparse(1:ne,C,ne-(1:ne)+1,ne,max(C));
    I = max(E2C,[],1);
    I = I(I>0);
    % reverse
    I = ne - I + 1;
  case 'recursive'
    keep = 1:size(E,1);
    Ekeep = E(keep,:);
    J = 1:size(E,1);
    I = [];
    while true
      % This should use random for large meshes
      [~,Ikeep] = conservative_edge_matching(Ekeep,'Method','random');
      I = [I J(Ikeep)];
      % Remove edges connected to E
      keep = find(~any(ismember(Ekeep,Ekeep(Ikeep,:)),2));
      if isempty(keep)
        break;
      end
      % Indices in original edges
      J = J(keep);
      Ekeep = Ekeep(keep,:);
    end

  case 'random'
    ns = size(E,1);
    while true
      I = randperm(size(E,1));
      I = I(1:ns);
      % Reduced graph of randomly selected edges
      [~,~,EI] = unique(E(I,:));
      % Check if every vertex is valence 1
      V = full(sparse(EI,1,1));
      if max(V) == 1
        break;
      end
      ns = floor(ns/2);
      if ns <= 1
        I = 1;
        break;
      end
    end
  end

  % Output on original graph
  EE = E_orig(I,:);
end


function test_suite = test_createRhombododecahedron
%TESTCREATERHOMBODODECAHEDRON  One-line description here, please.
%
%   output = testCreateRhombododecahedron(input)
%
%   Example
%   testCreateRhombododecahedron
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-12-07,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function testCreation(testCase) %#ok<*DEFNU>

createRhombododecahedron();

function testVEFCreation(testCase)

[v, e, f] = createRhombododecahedron();
testCase.assertTrue(~isempty(v));
testCase.assertTrue(~isempty(e));
testCase.assertTrue(~isempty(f));

[nv, ne, nf] = getMeshElementsNumber;
testCase.assertEqual([nv 3], size(v));
testCase.assertEqual([ne 2], size(e));
testCase.assertEqual(nf, length(f));

function testVFCreation(testCase)

[v, f] = createRhombododecahedron();
testCase.assertTrue(~isempty(v));
testCase.assertTrue(~isempty(f));

[nv, ne, nf] = getMeshElementsNumber; %#ok<ASGLU>
testCase.assertEqual([nv 3], size(v));
testCase.assertEqual(nf, length(f));

function testMeshCreation(testCase)

mesh = createRhombododecahedron();
testCase.assertTrue(isstruct(mesh));
testCase.assertTrue(isfield(mesh, 'vertices'));
testCase.assertTrue(isfield(mesh, 'edges'));
testCase.assertTrue(isfield(mesh, 'faces'));

[nv, ne, nf] = getMeshElementsNumber;
testCase.assertEqual([nv 3], size(mesh.vertices));
testCase.assertEqual([ne 2], size(mesh.edges));
testCase.assertEqual(nf, length(mesh.faces));

function testFacesOutwards(testCase)

[v, e, f] = createRhombododecahedron(); %#ok<ASGLU>

centro = centroid(v);
fc  = meshFaceCentroids(v, f);
fc2 = createVector(centro, fc);
n   = meshFaceNormals(v, f);

testCase.assertEqual(size(n), size(fc2));

dp = dot(fc2, n, 2);

testCase.assertTrue(sum(dp <= 0) == 0);

function [nv, ne, nf] = getMeshElementsNumber

nv = 14;
ne = 24;
nf = 12;


function writeEnergy(N,T,U)

%This function writes the energy file for the n-link pendulum.

nStateStr = num2str(2*N);

filename = ['energy_' num2str(N) '_link'];

comments{1} = ['[ENERGY, KINETIC, POTENTIAL] = ' upper(filename) '(Z,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the energy of a double';
comments{4} = '    pendulum.';
comments{5}  = 'INPUTS: ';
comments{10} = ['    z = [' nStateStr ' X nTime]  matrix of states'];
comments{11} = '    P = struct of parameters';
comments{8}  = 'OUTPUTS: ';
comments{9}  = '    energy = [1 X nTime] vector of total energy';
comments{10} = '    kinetic = [1 X nTime] vector of kinetic energy';
comments{11} = '    potential = [1 X nTime] vector of potential energy';
comments{12} = ' ';
comments{13} = 'NOTES:';
comments{14} = ['    This file was automatically generated by ' mfilename '.m'];

params{1} = {'g ','gravity'};
params{2} = {'m','mass'};
params{3} = {'l','length'};
params{4} = {'I','moment of inertia about its center of mass'};
params{5} = {'d','distance between center of mass and parent joint'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function [energy, kinetic, potential] = ' filename '(z,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    if i==1  %Gravity, scalar
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
    else  %unpack vectors:
        for j=1:N
           fprintf(fid,[params{i}{1} num2str(j) ...
               ' = P.' params{i}{1} '(' num2str(j) '); %% Link ' num2str(j) ' ' params{i}{2} '\n']);
        end
    end
end
fprintf(fid,'\n');

for j = 1:N
   fprintf(fid,['th' num2str(j) ' = z(' num2str(j) ',:); \n']);
end
fprintf(fid,'\n');

for j = 1:N
   fprintf(fid,['dth' num2str(j) ' = z(' num2str(j+N) ',:); \n']);
end
fprintf(fid,'\n');

fprintf(fid,['kinetic = ' vectorize(char(T)) ';\n']);
fprintf(fid,['potential = ' vectorize(char(U)) ';\n']);
fprintf(fid,'energy = potential + kinetic;\n\n');
fprintf(fid,'end \n');

fclose(fid);

end


function tests = VehicleTest
  tests = functiontests(localfunctions);
  clc
end

function Bicycle_constructor_test(tc)

    % default constructor
    v = Bicycle();

    % display
    v

    % char
    s = v.char()
    tc.verifyTrue( ischar(s) );

    % all options
    v = Bicycle( ...
        'steermax', 2, ...
        'accelmax', 3, ...
        'covar', [1 2;3 4], ...
        'speedmax', 5, ...
        'L',2.5, ...
        'x0', [1 2 3], ...
        'dt', 0.5, ...
        'rdim', 0.3 ...
        );

    tc.verifyEqual(v.steermax, 2);
    tc.verifyEqual(v.accelmax, 3);
    tc.verifyEqual(v.V, [1 2;3 4]);
    tc.verifyEqual(v.speedmax, 5);
    tc.verifyEqual(v.L, 2.5);
    tc.verifyEqual(v.x0, [1 2 3]');
    tc.verifyEqual(v.dt, 0.5);
    tc.verifyEqual(v.rdim, 0.3);

    v.init()
    tc.verifyEqual(v.x, [1 2 3]');

end

function Bicycle_deriv_test(tc)

    v = Bicycle('steermax', Inf);

    xd =v.deriv([], [0 0 0], [1 0]);
    tc.verifyEqual( xd, [1 0 0]', 'AbsTol', 1e-6);

    xd =v.deriv([], [0 0 pi/2], [1 0]);
    tc.verifyEqual( xd, [0 1 0]', 'AbsTol', 1e-6);

    xd =v.deriv([], [0 0 0], [0 1]);
    tc.verifyEqual( xd, [0 0 0]', 'AbsTol', 1e-6);

    xd =v.deriv([], [0 0 0], [1 pi/4]);
    tc.verifyEqual( xd, [1 0 1]', 'AbsTol', 1e-6);

    v = Bicycle('steermax', pi/4);
    xd =v.deriv([], [0 0 0], [1 100]);
    tc.verifyEqual( xd, [1 0 1]', 'AbsTol', 1e-6);

    v = Bicycle('speedmax', 1, 'steermax', Inf);
    xd =v.deriv([], [0 0 0], [100 pi/4]);
    tc.verifyEqual( xd, [1 0 1]', 'AbsTol', 1e-6);

    v = Bicycle('accelmax', 1);
    xd =v.deriv([], [0 0 0], [100 0]);
    tc.verifyEqual( xd, [v.dt 0 0]', 'AbsTol', 1e-6);
    for i=1:9
            xd =v.deriv([], [0 0 0], [100 0]);
    end
    tc.verifyEqual( xd, [1 0 0]', 'AbsTol', 1e-6);

end

function Bicycle_update_test(tc)

    v = Bicycle('dt', 1, 'speedmax', Inf, 'steermax', Inf);

    tc.verifyEqual( v.update([1 0]), [1 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.x, [1 0 0]', 'AbsTol', 1e-6);

    tc.verifyEqual( v.update([1 0]), [1 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.x, [2 0 0]', 'AbsTol', 1e-6);

    tc.verifyEqual( v.update([-2 0]), [2 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.x, [0 0 0]', 'AbsTol', 1e-6);

    tc.verifyEqual( size(v.x_hist,1), 3);

    v.init();
    tc.verifyEqual( size(v.x_hist,1), 0);

    tc.verifyEqual( v.update([0 1]), [0 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.update([1 pi/4]), [1 1], 'AbsTol', 1e-6);
end

function Bicycle_f_test(tc)

    v = Bicycle('steermax', Inf);

    tc.verifyEqual( v.f([0 0 0], [1 0], [0 0]), [1 0 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.f([2 3 0], [1 0], [0 0]), [3 3 0], 'AbsTol', 1e-6);

    tc.verifyEqual( v.f([0 0 0], [0 0], [1 0]), [1 0 0], 'AbsTol', 1e-6);

    tc.verifyEqual( v.f([0 0 0], [0 1], [0 0]), [0 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.f([0 0 0], [sqrt(2) pi/4], [0 0]), [1 1 pi/4], 'AbsTol', 1e-6);
    tc.verifyEqual( v.f([0 0 0], [sqrt(2) 0], [0 pi/4]), [1 1 pi/4], 'AbsTol', 1e-6);
end

function Bicycle_jacobian_test(tc)

    v = Bicycle('steermax', Inf);

    tc.verifyEqual( v.Fx([0 0 0], [0 0]), [1 0 0; 0 1 0; 0 0 1], 'AbsTol', 1e-6);

    tc.verifyEqual( v.Fx([0 0 0], [2 0]), [1 0 0; 0 1 2; 0 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fx([2 3 0], [2 0]), [1 0 0; 0 1 2; 0 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fx([0 0 0], [3 pi/2]), [1 0 -3; 0 1 0; 0 0 1], 'AbsTol', 1e-6);

    tc.verifyEqual( v.Fv([0 0 0], [0 0]), [1 0; 0 0; 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fv([2 3 0], [0 0]), [1 0; 0 0; 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fv([0 0 0], [2 3]), [1 0; 0 0; 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fv([0 0 pi/2], [0 0]), [0 0; 1 0; 0 1], 'AbsTol', 1e-6);
end

function Unicycle_constructor_test(tc)

    % default constructor
    v = Unicycle();

    % display
    v

    % char
    s = v.char()
    tc.verifyTrue( ischar(s) );

    % all options
    v = Bicycle( ...
        'accelmax', 3, ...
        'covar', [1 2;3 4], ...
        'speedmax', 5, ...
        'L',2.5, ...
        'x0', [1 2 3], ...
        'dt', 0.5, ...
        'rdim', 0.3 ...
        );

    tc.verifyEqual(v.accelmax, 3);
    tc.verifyEqual(v.V, [1 2;3 4]);
    tc.verifyEqual(v.speedmax, 5);
    tc.verifyEqual(v.L, 2.5);
    tc.verifyEqual(v.x0, [1 2 3]');
    tc.verifyEqual(v.dt, 0.5);
    tc.verifyEqual(v.rdim, 0.3);

    v.init()
    tc.verifyEqual(v.x, [1 2 3]');
end

function Unicycle_deriv_test(tc)

    v = Unicycle();

    xd =v.deriv([], [0 0 0], [1 0]);
    tc.verifyEqual( xd, [1 0 0]', 'AbsTol', 1e-6);

    xd =v.deriv([], [0 0 pi/2], [1 0]);
    tc.verifyEqual( xd, [0 1 0]', 'AbsTol', 1e-6);

    xd =v.deriv([], [0 0 0], [0 1]);
    tc.verifyEqual( xd, [0 0 1]', 'AbsTol', 1e-6);

    xd =v.deriv([], [0 0 0], [1 1]);
    tc.verifyEqual( xd, [1 0 1]', 'AbsTol', 1e-6);

    v = Unicycle('speedmax', 1);
    xd =v.deriv([], [0 0 0], [100 0]);
    tc.verifyEqual( xd, [1 0 0]', 'AbsTol', 1e-6);

    v = Unicycle('accelmax', 1);
    xd =v.deriv([], [0 0 0], [100 0]);
    tc.verifyEqual( xd, [v.dt 0 0]', 'AbsTol', 1e-6);
    for i=1:9
            xd =v.deriv([], [0 0 0], [100 0]);
    end
    tc.verifyEqual( xd, [1 0 0]', 'AbsTol', 1e-6);

end

function Unicycle_update_test(tc)

    v = Unicycle('dt', 1, 'speedmax', Inf);

    tc.verifyEqual( v.update([1 0]), [1 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.x, [1 0 0]', 'AbsTol', 1e-6);

    tc.verifyEqual( v.update([1 0]), [1 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.x, [2 0 0]', 'AbsTol', 1e-6);

    tc.verifyEqual( v.update([-2 0]), [2 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.x, [0 0 0]', 'AbsTol', 1e-6);

    tc.verifyEqual( size(v.x_hist,1), 3);

    v.init();
    tc.verifyEqual( size(v.x_hist,1), 0);

    tc.verifyEqual( v.update([0 1]), [0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.update([2 1]), [2 1], 'AbsTol', 1e-6);
end

function Unicycle_f_test(tc)

    v = Unicycle();

    tc.verifyEqual( v.f([0 0 0], [1 0], [0 0]), [1 0 0], 'AbsTol', 1e-6);
    tc.verifyEqual( v.f([2 3 0], [1 0], [0 0]), [3 3 0], 'AbsTol', 1e-6);

    tc.verifyEqual( v.f([0 0 0], [0 0], [1 0]), [1 0 0], 'AbsTol', 1e-6);

    tc.verifyEqual( v.f([0 0 0], [0 1], [0 0]), [0 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.f([0 0 0], [sqrt(2) pi/4], [0 0]), [1 1 pi/4], 'AbsTol', 1e-6);
    tc.verifyEqual( v.f([0 0 0], [sqrt(2) 0], [0 pi/4]), [1 1 pi/4], 'AbsTol', 1e-6);
end

function Unicycle_jacobian_test(tc)

    v = Unicycle();

    tc.verifyEqual( v.Fx([0 0 0], [0 0]), [1 0 0; 0 1 0; 0 0 1], 'AbsTol', 1e-6);

    tc.verifyEqual( v.Fx([0 0 0], [2 0]), [1 0 0; 0 1 2; 0 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fx([2 3 0], [2 0]), [1 0 0; 0 1 2; 0 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fx([0 0 0], [3 pi/2]), [1 0 -3; 0 1 0; 0 0 1], 'AbsTol', 1e-6);

    tc.verifyEqual( v.Fv([0 0 0], [0 0]), [1 0; 0 0; 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fv([2 3 0], [0 0]), [1 0; 0 0; 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fv([0 0 0], [2 3]), [1 0; 0 0; 0 1], 'AbsTol', 1e-6);
    tc.verifyEqual( v.Fv([0 0 pi/2], [0 0]), [0 0; 1 0; 0 1], 'AbsTol', 1e-6);
end


function p = prior_t(varargin)
%PRIOR_T  Student-t prior structure
%
%  Description
%    P = PRIOR_T('PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    creates Student's t-distribution prior structure in which the
%    named parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    P = PRIOR_T(P, 'PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    modify a prior structure with the named parameters altered
%    with the specified values.
%
%    The parameterization is as in Gelman, Carlin, Stern, Dunson, Vehtari,
%    and Rubin (2013). Bayesian Data Analysis, third edition.
%
%    Parameters for Student-t prior [default]
%      mu       - location [0]
%      s2       - scale [1]
%      nu       - degrees of freedom [4]
%      mu_prior - prior for mu [prior_fixed]
%      s2_prior - prior for s2 [prior_fixed]
%      nu_prior - prior for nu [prior_fixed]
%
%  See also
%    PRIOR_*
%
% Copyright (c) 2000-2001,2010 Aki Vehtari
% Copyright (c) 2009 Jarno Vanhatalo
% Copyright (c) 2010 Jaakko Riihimäki

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'PRIOR_T';
  ip.addOptional('p', [], @isstruct);
  ip.addParamValue('mu',0, @(x) isscalar(x));
  ip.addParamValue('mu_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('s2',1, @(x) isscalar(x) && x>0);
  ip.addParamValue('s2_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('nu',4, @(x) isscalar(x) && x>0);
  ip.addParamValue('nu_prior',[], @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 't';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'t')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('mu',ip.UsingDefaults)
    p.mu = ip.Results.mu;
  end
  if init || ~ismember('s2',ip.UsingDefaults)
    p.s2 = ip.Results.s2;
  end
  if init || ~ismember('nu',ip.UsingDefaults)
    p.nu = ip.Results.nu;
  end
  % Initialize prior structure
  if init
    p.p=[];
  end
  if init || ~ismember('mu_prior',ip.UsingDefaults)
    p.p.mu=ip.Results.mu_prior;
  end
  if init || ~ismember('s2_prior',ip.UsingDefaults)
    p.p.s2=ip.Results.s2_prior;
  end
  if init || ~ismember('nu_prior',ip.UsingDefaults)
    p.p.nu=ip.Results.nu_prior;
  end

  if init
    % set functions
    p.fh.pak = @prior_t_pak;
    p.fh.unpak = @prior_t_unpak;
    p.fh.lp = @prior_t_lp;
    p.fh.lpg = @prior_t_lpg;
    p.fh.recappend = @prior_t_recappend;
  end

end

function [w, s, h] = prior_t_pak(p)
% This is a mandatory subfunction used for example
% in energy and gradient computations.

  w=[];
  s={};
  h=[];
  if ~isempty(p.p.mu)
    w = p.mu;
    s=[s; 't.mu'];
    h = 1;
  end
  if ~isempty(p.p.s2)
    w = [w log(p.s2)];
    s=[s; 'log(t.s2)'];
    h = [h 1];
  end
  if ~isempty(p.p.nu)
    w = [w log(p.nu)];
    s=[s; 'log(t.nu)'];
    h = [h 1];
  end
end

function [p, w] = prior_t_unpak(p, w)
% This is a mandatory subfunction used for example
% in energy and gradient computations.

  if ~isempty(p.p.mu)
    i1=1;
    p.mu = w(i1);
    w = w(i1+1:end);
  end
  if ~isempty(p.p.s2)
    i1=1;
    p.s2 = exp(w(i1));
    w = w(i1+1:end);
  end
  if ~isempty(p.p.nu)
    i1=1;
    p.nu = exp(w(i1));
    w = w(i1+1:end);
  end
end

function lp = prior_t_lp(x, p)
% This is a mandatory subfunction used for example
% in energy computations.

  lp=sum(gammaln((p.nu+1)./2) -gammaln(p.nu./2) -0.5*log(p.nu.*pi.*p.s2) -(p.nu+1)./2.*log(1+(x-p.mu).^2./p.nu./p.s2));

  if ~isempty(p.p.mu)
    lp = lp + p.p.mu.fh.lp(p.mu, p.p.mu);
  end
  if ~isempty(p.p.s2)
    lp = lp + p.p.s2.fh.lp(p.s2, p.p.s2) +log(p.s2);
  end
  if ~isempty(p.p.nu)
    lp = lp + p.p.nu.fh.lp(p.nu, p.p.nu) +log(p.nu);
  end
end

function lpg = prior_t_lpg(x, p)
% This is a mandatory subfunction used for example
% in gradient computations.

 %lpg=(p.nu+1)./p.nu .* (x-p.mu)./p.s2 ./ (1 + (x-p.mu).^2./p.nu./p.s2);
  lpg=-(p.nu+1).* (x-p.mu) ./ (p.nu.*p.s2 + (x-p.mu).^2);

  if ~isempty(p.p.mu)
    lpgmu = sum( (p.nu+1).* (x-p.mu) ./ (p.nu.*p.s2 + (x-p.mu).^2) ) + p.p.mu.fh.lpg(p.mu, p.p.mu);
    lpg = [lpg lpgmu];
  end
  if ~isempty(p.p.s2)
    lpgs2 = (sum( -1./(2.*p.s2) +((p.nu + 1).*(p.mu - x).^2)./(2.*p.s2.*((p.mu-x).^2 + p.nu.*p.s2))) + p.p.s2.fh.lpg(p.s2, p.p.s2)).*p.s2 + 1;
    lpg = [lpg lpgs2];
  end
  if ~isempty(p.p.nu)
    lpgnu = (0.5*sum( digamma1((p.nu+1)./2)-digamma1(p.nu./2)-1./p.nu-log(1+(x-p.mu).^2./p.nu./p.s2)+(p.nu+1)./(1+(x-p.mu).^2./p.nu./p.s2).*(x-p.mu).^2./p.s2./p.nu.^2) + p.p.nu.fh.lpg(p.nu, p.p.nu)).*p.nu + 1;
    lpg = [lpg lpgnu];
  end
end

function rec = prior_t_recappend(rec, ri, p)
% This subfunction is needed when using MCMC sampling (gp_mc).

% The parameters are not sampled in any case.
  rec = rec;
  if ~isempty(p.p.mu)
    rec.mu(ri,:) = p.mu;
  end
  if ~isempty(p.p.s2)
    rec.s2(ri,:) = p.s2;
  end
  if ~isempty(p.p.nu)
    rec.nu(ri,:) = p.nu;
  end
end


function h = plotaxes(a, m, type, col, varargin)
% PLOTAXES  Plot axes of 3D coordinate system
%
% PLOTAXES(A, M, TYPE, COL)
%
%   A is a (3,3)-matrix with 3 column vectors. Each vector gives the
%   direction of one axis of a Coordinate system.
%
%   M is a 3-vector with the origin of coordinates. By default, M=[0,0,0].
%
%   TYPE is a cell array with the line types to plot (e.g. '--', ':'... see
%   "help plot" for more details). By default, type='-', i.e. a solid line.
%
%   COL is a cell array with the colours of each axis. By default,
%   col={'b', 'r', 'g'}. If only one string is provided, all axes are
%   displayed with the same colour.
%
% PLOTAXES(A, M, TYPE, COL, ...)
%
%   ... Any extra input arguments will be passed to the plot function.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2010-2011 University of Oxford
% Version: 0.2.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
error(nargchk(1, Inf, nargin, 'struct'));
error(nargoutchk(0, 1, nargout, 'struct'));

% defaults
if (nargin < 2 || isempty(m))
    m = [0 0 0];
end
if (nargin < 3 || isempty(type))
    type = {'-', '-', '-'};
end
if (nargin < 4 || isempty(col))
    col = {'b', 'r', 'g'};
end
if (~iscell(type) || length(type) == 1)
    if iscell(type)
        type = type{:};
    end
    type = {type, type, type};
end
if (~iscell(col) || length(col) == 1)
    if iscell(col)
        col = col{:};
    end
    col = {col, col, col};
end

% save hold state for later
PHOLD = ishold;

% plot axes
for I = 1:size(a, 2)
    h = plot3(m(1) + [0, a(1, I)], ...
        m(2) + [0, a(2, I)], m(3) + [0, a(3, I)], [ type{I} col{I} ], ...
        varargin{:});
    hold on
end

% recovert hold state
if PHOLD
    hold on
else
    hold off
end


function Yout = prtUtilEvalCAPtree(tree,X,nClasses)
%Yout = evalCAPtree(tree,X)
%   Evaluate a CAP tree on a 1xN data point X
% Internal
% xxx Need Help xxx

if nargin < 3 || isempty(nClasses)
    nClasses = 2;
end

index = 1;
voted = false;
while ~voted
    if any(isfinite(tree.W(:,index)))
        %disp(((tree.W(:,index)'*X(:,tree.featureIndices(:,index))')') - tree.threshold(:,index))
        Yout = double(((tree.W(:,index)'*X(:,tree.featureIndices(:,index))')' - tree.threshold(:,index)) >= 0);
        if Yout == 0
            index = find(tree.treeIndices(:) == index,1,'first');
        elseif Yout > 0
            index = find(tree.treeIndices(:) == index,1,'last');
        end
    else
        Yout = zeros(1,nClasses);
        Yout(tree.terminalVote(index)) = 1;
        voted = true;
    end
end


function [ net ] = InitFeatureExtractionNet( net,net1)

net.addLayer('conv1', dagnn.Conv('size', [3,3,3,64], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'input', 'conv_1', {'conv1_f', 'conv1_b'});
net.addLayer('relu1', dagnn.ReLU(), 'conv_1', 'relu_1');

f=net.getParamIndex('conv1_f');
net.params(f).value=net1.layers{1}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv1_b');
net.params(f).value=net1.layers{1}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv2', dagnn.Conv('size', [3,3,64,64], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_1', 'conv_2', {'conv2_f', 'conv2_b'});
net.addLayer('relu2', dagnn.ReLU(), 'conv_2', 'relu_2');

f=net.getParamIndex('conv2_f');
net.params(f).value=net1.layers{3}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv2_b');
net.params(f).value=net1.layers{3}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('pool1', dagnn.Pooling('poolSize', [2,2], 'pad', [0,0,0,0],...
    'stride', [2,2]),'relu_2','pool_1');

%----------------------------------
net.addLayer('conv3', dagnn.Conv('size', [3,3,64,128], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_1', 'conv_3', {'conv3_f', 'conv3_b'});
net.addLayer('relu3', dagnn.ReLU(), 'conv_3', 'relu_3');

f=net.getParamIndex('conv3_f');
net.params(f).value=net1.layers{6}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv3_b');
net.params(f).value=net1.layers{6}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv4', dagnn.Conv('size', [3,3,128,128], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_3', 'conv_4', {'conv4_f', 'conv4_b'});
net.addLayer('relu4', dagnn.ReLU(), 'conv_4', 'relu_4');

f=net.getParamIndex('conv4_f');
net.params(f).value=net1.layers{8}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv4_b');
net.params(f).value=net1.layers{8}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('pool2', dagnn.Pooling('poolSize', [2,2], 'pad', [0,0,0,0],...
    'stride', [2,2]),'relu_4','pool_2');

%------------------------------
net.addLayer('conv5', dagnn.Conv('size', [3,3,128,256], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_2', 'conv_5', {'conv5_f', 'conv5_b'});
net.addLayer('relu5', dagnn.ReLU(), 'conv_5', 'relu_5');

f=net.getParamIndex('conv5_f');
net.params(f).value=net1.layers{11}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv5_b');
net.params(f).value=net1.layers{11}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv6', dagnn.Conv('size', [3,3,256,256], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_5', 'conv_6', {'conv6_f', 'conv6_b'});
net.addLayer('relu6', dagnn.ReLU(), 'conv_6', 'relu_6');

f=net.getParamIndex('conv6_f');
net.params(f).value=net1.layers{13}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv6_b');
net.params(f).value=net1.layers{13}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv7', dagnn.Conv('size', [3,3,256,256], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_6', 'conv_7', {'conv7_f', 'conv7_b'});
net.addLayer('relu7', dagnn.ReLU(), 'conv_7', 'relu_7');

f=net.getParamIndex('conv7_f');
net.params(f).value=net1.layers{15}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv7_b');
net.params(f).value=net1.layers{15}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv8', dagnn.Conv('size', [3,3,256,256], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_7', 'conv_8', {'conv8_f', 'conv8_b'});
net.addLayer('relu8', dagnn.ReLU(), 'conv_8', 'relu_8');

f=net.getParamIndex('conv8_f');
net.params(f).value=net1.layers{17}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv8_b');
net.params(f).value=net1.layers{17}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('pool3', dagnn.Pooling('poolSize', [2,2], 'pad', [0,0,0,0],...
    'stride', [2,2]),'relu_8','pool_3');

net.addLayer('conv9', dagnn.Conv('size', [3,3,256,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_3', 'conv_9', {'conv9_f', 'conv9_b'});
net.addLayer('relu9', dagnn.ReLU(), 'conv_9', 'relu_9');

f=net.getParamIndex('conv9_f');
net.params(f).value=net1.layers{20}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv9_b');
net.params(f).value=net1.layers{20}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv10', dagnn.Conv('size', [3,3,512,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_9', 'conv_10', {'conv10_f', 'conv10_b'});
net.addLayer('relu10', dagnn.ReLU(), 'conv_10', 'relu_10');

f=net.getParamIndex('conv10_f');
net.params(f).value=net1.layers{22}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv10_b');
net.params(f).value=net1.layers{22}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv11', dagnn.Conv('size', [3,3,512,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_10', 'conv_11', {'conv11_f', 'conv11_b'});
net.addLayer('relu11', dagnn.ReLU(), 'conv_11', 'relu_11');

f=net.getParamIndex('conv11_f');
net.params(f).value=net1.layers{24}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv11_b');
net.params(f).value=net1.layers{24}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv12', dagnn.Conv('size', [3,3,512,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_11', 'conv_12', {'conv12_f', 'conv12_b'});
net.addLayer('relu12', dagnn.ReLU(), 'conv_12', 'relu_12');

f=net.getParamIndex('conv12_f');
net.params(f).value=net1.layers{26}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv12_b');
net.params(f).value=net1.layers{26}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('pool4', dagnn.Pooling('poolSize', [2,2], 'pad', [0,0,0,0],...
    'stride', [2,2]),'relu_12','pool_4');

net.addLayer('conv13', dagnn.Conv('size', [3,3,512,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_4', 'conv_13', {'conv13_f', 'conv13_b'});
net.addLayer('relu13', dagnn.ReLU(), 'conv_13', 'relu_13');

f=net.getParamIndex('conv13_f');
net.params(f).value=net1.layers{29}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv13_b');
net.params(f).value=net1.layers{29}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv14', dagnn.Conv('size', [3,3,512,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_13', 'conv_14', {'conv14_f', 'conv14_b'});
net.addLayer('relu14', dagnn.ReLU(), 'conv_14', 'relu_14');

f=net.getParamIndex('conv14_f');
net.params(f).value=net1.layers{31}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv14_b');
net.params(f).value=net1.layers{31}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv15', dagnn.Conv('size', [3,3,512,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_14', 'conv_15', {'conv15_f', 'conv15_b'});
net.addLayer('relu15', dagnn.ReLU(), 'conv_15', 'relu_15');

f=net.getParamIndex('conv15_f');
net.params(f).value=net1.layers{33}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv15_b');
net.params(f).value=net1.layers{33}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('conv16', dagnn.Conv('size', [3,3,512,512], 'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_15', 'conv_16', {'conv16_f', 'conv16_b'});
net.addLayer('relu16', dagnn.ReLU(), 'conv_16', 'relu_16');

f=net.getParamIndex('conv16_f');
net.params(f).value=net1.layers{35}.filters;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

f=net.getParamIndex('conv16_b');
net.params(f).value=net1.layers{35}.biases;
net.params(f).learningRate=0;
net.params(f).weightDecay=0;

net.addLayer('pool5', dagnn.Pooling('poolSize', [2,2], 'pad', [0,0,0,0],...
    'stride', [2,2]),'relu_16','pool_5');

end


function check = uniform_discrete_check ( a, b )

%*****************************************************************************80
%
%% UNIFORM_DISCRETE_CHECK checks the parameters of the Uniform discrete CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, B, the parameters of the PDF.
%    A <= B.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  if ( b < a )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'UNIFORM_DISCRETE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B < A.\n' );
    check = 0;
  end

  check = 1;

  return
end


function [M_V] = vox2ras_ksolve(M_R, Vc_Ps, varargin)
%%
%% NAME
%%
%%     vox2ras_ksolve.m (vox2ras_k{-space column}solve)
%%
%% AUTHOR
%%
%%	Rudolph Pienaar
%%
%% SYNOPSIS
%%
%%     [M_V] = vox2ras_ksolve(M_R, Vc_Ps, Vr_logicalSpace)
%%
%% ARGUMENTS
%%
%%	M_R		in	4x4 vox2ras candidate matrix with correct
%%					direction cosines, but invalid
%%					k-space column
%%      Vc_Ps		in      column vector defining the slice position
%%					(typically read from meas.asc)
%%	Vr_logicalSpace	in/opt	row vector that defines the logical dimension
%%					size. If omitted, this is assumed
%%					to be [256 256 128]
%%	M_V		out	complete 4x4 vox2ras matrix
%%
%% DESCRIPTION
%%
%%	"vox2ras_ksolve" uses the given rotation components of M_R and the
%%	starting point, Vc_Ps, to determine the center of k-space and thus
%%	create a fully-qualified vox2ras matrix.
%%
%% PRECONDITIONS
%%
%%	o M_R is 4x4 where the first 3x3 submatrix contains the direction
%%		cosines. The 4th column is ignored, as is the 4th row.
%%	o The vector Vc_Ps is typically read from a Siemens meas.asc file such
%%	  that
%%		Vc_Ps(1)	= sSliceArray.asSlice[0].sPosition.dSag
%%		Vc_Ps(2)	= sSliceArray.asSlice[0].sPosition.dCor
%%		Vc_ps(3)	= sSliceArray.asSlice[0].sPosition.dTra
%%
%% POSTCONDITIONS
%%
%%	o M_V(1:3, 1:3) = M_R(1:3, 1:3)
%%	o M_V(:,4)	= k-space center
%%
%% SEE ALSO
%%
%%	vox2ras_rsolveAA- determine the rotational component of a vox2ras matrix
%%				using Siemens reference orientations directly
%%	vox2ras_rsolve	- determine the rotational component of a vox2ras matrix
%%				using Siemens reference orientations indirectly
%%	vox2ras_dfmeas	- main function: determines the vox2ras matrix from a
%%			  Siemens meas.asc file.
%%
%% HISTORY
%%
%% 26 May 2004
%% o Initial design and coding.
%%

%
% vox2ras_ksolve.m
%
% Original Author: Rudolph Pienaar
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

M_V		= M_R;
M_V(4,:)	= M_R(4,:)*0;
M_V(:,4)	= M_R(:,4)*0;
M_V(4,4)	= 1;

Vr_logicalSpace	= [ 256 256 128 ];
if length(varargin)
	Vr_logicalSpace = varargin{1};
end

%% First read the direction cosines
Vc_x 		= M_R(1:3,1);
Vc_y 		= M_R(1:3,2);
Vc_z 		= M_R(1:3,3);

xoff		= Vr_logicalSpace(1)/2;
yoff		= Vr_logicalSpace(2)/2;
zoff		= Vr_logicalSpace(3)/2;

%% Solve for the k-space center:
%% The first two components are found by moving along the direction cosines
Vc_Pe1		=  - (Vc_Ps + (xoff*Vc_x + yoff*Vc_y + zoff*Vc_z));
%% and the last component is found by moving against the direction cosines
Vc_Pe2		= (Vc_Ps - (xoff*Vc_x + yoff*Vc_y + zoff*Vc_z));
Vc_Pe(1)	= Vc_Pe1(1) -0.67;		%% a strange correction?
Vc_Pe(2)	= Vc_Pe1(2);
Vc_Pe(3)	= Vc_Pe2(3);
M_V(1:3, 4)	= Vc_Pe';

%% All done!


function [GAmp,GTime]=GxEPI(p)

global VCtl;
global VObj;
global VVar;

t1Start=p.t1Start;
t2Middle=p.t2Middle;
Gx1Sign=p.Gx1Sign;
Gx2Sign=p.Gx2Sign;

% frequency encoding
KxMax=(1/VCtl.RFreq)/2;

% prephasing lob
p.tStart=t1Start;
p.Area=KxMax;
p.Duplicates=1;
p.DupSpacing=0;
[GAmp1,GTime1]=GxAreaTrapezoid2(p);
p=[];

% readout lobs
p.GxAmp=(1/VCtl.FOVFreq)/((VObj.Gyro/(2*pi))*(1/VCtl.BandWidth));
tHalf=1/(2*(VObj.Gyro/(2*pi))*p.GxAmp*VCtl.RFreq);

TimeOffset = (t2Middle+VCtl.TEAnchorTime)-(floor(VCtl.EPI_ETL/2)+0.5)*VCtl.EPI_ESP;
if strcmp(VCtl.EPI_EchoShifting,'on')
     TimeOffset = TimeOffset + (VVar.PhaseCount-1)*(VCtl.EPI_ESP/VCtl.EPI_ShotNum);
end
p.tRamp = VCtl.EPI_ESP/2 - tHalf;
p.sRamp = 2;
p.Duplicates=1;
p.DupSpacing=0;
GAmp2=[];
GTime2=[];
for i = 1: VCtl.EPI_ETL
    p.tStart = TimeOffset + (i-1)*VCtl.EPI_ESP + p.tRamp;
    p.tEnd = p.tStart + 2 * tHalf;
    [GAmpt,GTimet]=GxTrapezoid(p);
    GAmp2=[GAmp2 GAmpt*(-1)^(i-1)];
    GTime2=[GTime2 GTimet];
end

GAmp=[GAmp1*Gx1Sign GAmp2*Gx2Sign];
GTime=[GTime1 GTime2];

[GTime,m,n]=unique(GTime);
GAmp=GAmp(m);

end


function [lores hires] = collectSamplesScales(conf, ohires, numscales, scalefactor)

lores = [];
hires = [];

for scale = 1:numscales
    sfactor = scalefactor^(scale-1);
    chires = resize(ohires, sfactor, 'bicubic');

    chires = modcrop(chires, conf.scale); % crop a bit (to simplify scaling issues)
    % Scale down images
    clores = resize(chires, 1/conf.scale, conf.interpolate_kernel);
    midres = resize(clores, conf.upsample_factor, conf.interpolate_kernel);
    features = collect(conf, midres, conf.upsample_factor, conf.filters);
    clear midres

    interpolated = resize(clores, conf.scale, conf.interpolate_kernel);
    clear clores
    patches = cell(size(chires));
    for i = 1:numel(patches) % Remove low frequencies
        patches{i} = chires{i} - interpolated{i};
    end
    clear chires interpolated

    hires = [hires collect(conf, patches, conf.scale, {})];

    lores = [lores conf.V_pca' * features];
end


filename='Cantilever_tetrahedra';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.5;
optimality_final =1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function [X, Y, B] = ginput_datetime(ax, varargin)
    % standard ginput, but can handle date_time. Axes must be specified
    % [x, y, button] = ginput_datetime(ax, ...)
    %
    % because, for large date ranges, there is an artificial level of precision,
    % the addition of a 'nearest_...' parameter (as the last parameter) will do rounding
    %
    % [x, y, button] = ginput_datetime(ax, ... , 'nearest_day')
    % [x, y, button] = ginput_datetime(ax, ... , 'nearest_hour')
    %
    %
    % see also ginput
    %
    % Celso G Reyes, 2017

    selector = '';
    if ~isempty(varargin) && ischar(varargin{end}) && startsWith(varargin{end},'nearest_')
        selector = varargin{end};
        varargin(end) = [];
    end

    axes(ax);
    [X, Y, B] = ginput(varargin{:});

    X = num2ruler(X, ax.XAxis);
    Y = num2ruler(Y, ax.YAxis);
    if isa(X,'datetime')
        X = round_time(X,selector);
    end
    if isa(Y,'datetime')
        Y = round_time(Y,selector);
    end

end

function dates = round_time(dates, selector)
    switch selector
        case 'nearest_day'
            dates = datetime(dates.Year, dates.Month, dates.Day + round(dates.Hour ./ 24));
        case 'nearest_hour'
            dates = datetime(dates.Year, dates.Month, dates.Day, dates.Hour + round(dates.Minute/60),0,0);
        otherwise
            % do nothing
    end
end


function varargout = surface( f, varargin )
%SURFACE  Plot surface of a SEPARABLEAPPROX.
%   SURFACE(X, Y, Z, C) adds the surface in X,Y,Z,C to the current axes.
%
%   SURFACE(X, Y, Z) uses C = Z, so color is proportional to surface height.
%
%   See SURF for a complete description of the various forms that X,Y,Z,C can
%   take.
%
% See also SURF.

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

if ( isempty( f ) )
     surface( [ ] );
     return
end

holdState = ishold;
numpts = 200;
dom = f.domain;

% Data points:
x = linspace(dom(1), dom(2), numpts);
y = linspace(dom(3), dom(4), numpts);
[xx, yy] = meshgrid( x, y );
val = feval(f, xx, yy);

% Options:
defaultopts = {'facecolor', 'interp', 'edgealpha', .5, 'edgecolor', 'none'};

if ( isempty(varargin) )
    h1 = surface( xx, yy, val, defaultopts{:} );
    hold on
    h2 = surface( xx.', yy.', val.', defaultopts{:} );
else
    h1 = surface( xx, yy, val, defaultopts{:}, varargin{:} );
    hold on
    h2 = surface( xx.', yy.', val.', defaultopts{:}, varargin{:} );
end

if ( ~holdState )
    hold off
end

if ( nargout > 0 )
    varargout = {h1 h2};
    return
end

end


%VGG_SEGMENT_MS  Mean shift image segmentation
%
%   S = vgg_segment_ms(A, h_s, h_r, min_sz[, W])
%
% Segmentation of an image using mean shift.
%
% Uses EDISON code by Chris Christoudias and Bogdan Georgescu, downloaded
% from http://www.caip.rutgers.edu/riul/research/code/EDISON/index.html,
% and based on the following papers:
%   [1] D. Comanicu, P. Meer: "Mean shift: A robust approach toward feature
%   space analysis". IEEE Trans. Pattern Anal. Machine Intell., May 2002.
%   [2] C. Christoudias, B. Georgescu, P. Meer: "Synergism in low level
%   vision". 16th International Conference of Pattern Recognition, Track 1
%   - Computer Vision and Robotics, Quebec City, Canada, August 2001.
%
%IN:
%   A - HxWx3 uint8 image for segmentation.
%   h_s - scalar parameter on.
%   h_r - scalar parameter on.
%   min_sz - scalar indicating the minimum number of pixels per segment.
%   W - HxW single matrix containing weights from edge detection for
%       synergism with segmentation. See [2]. Default: uniform weight.
%
%OUT:
%   S - HxW uint32 segmentation matrix, each value of which gives the index
%       of the region said pixel belongs to, from 1 to num_segments.

% $Id: vgg_segment_ms.m,v 1.1 2007/12/10 10:59:31 ojw Exp $

function varargout = vgg_segment_ms(varargin)
funcName = mfilename;
sd = 'seg_ms/';
sourceList = {['-I' sd], [funcName '.cxx'], [sd 'msImageProcessor.cpp'],...
              [sd 'ms.cpp'], [sd 'rlist.cpp'], [sd 'RAList.cpp'],...
              [sd 'msSys.cpp']};
vgg_mexcompile_script; % Compilation happens in this script
return


function [mindcf,pmiss,pfa] = plot_mindcf_point(plot_obj,target_prior,plot_args,legend_string)
% Places the mindcf point for the current system.
% Inputs:
%   target_prior: The effective target prior.
%   plot_args: A cell array of arguments to be passed to 'plot' that control
%     the appearance of the curve. See Matlab's help on 'plot' for information.
%   legend_string: Optional. A string to describe this curve in the legend.

if ischar(plot_args)
    plot_args = {plot_args};
end

[mindcf,pmiss,pfa] = fast_minDCF(plot_obj.tar,plot_obj.non,logit(target_prior),true);
if (pfa < plot_obj.pfa_limits(1)) || (pfa > plot_obj.pfa_limits(2))
    log_warning('pfa of %f is not between %f and %f. The mindcf point will not be plotted.\n',pfa,plot_obj.pfa_limits(1),plot_obj.pfa_limits(2))
elseif (pmiss < plot_obj.pmiss_limits(1)) || (pmiss > plot_obj.pmiss_limits(2))
    log_warning('pmiss of %f is not between %f and %f. The mindcf point will not be plotted.\n',pmiss,plot_obj.pmiss_limits(1),plot_obj.pmiss_limits(2))
else
    figure(plot_obj.fh);
    assert(iscell(plot_args))
    lh = plot(probit(pfa),probit(pmiss),plot_args{:});
    if exist('legend_string','var') && ~isempty(legend_string)
	assert(ischar(legend_string))
	plot_obj.add_legend_entry(lh,legend_string,true);
    end
end

end


%%*****************************************************************
%% checkdense : identify the dense columns of a matrix
%%
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function  idxden = checkdense(A);

   [m,n] = size(A);
   idxden = [];
   nzratio = 1;
   if (m > 1000); nzratio = 0.20; end;
   if (m > 2000); nzratio = 0.10; end;
   if (m > 5000); nzratio = 0.05; end;
   if (nzratio < 1)
      nzcolA = sum(spones(A));
      idxden = find(nzcolA > nzratio*m);
      if (length(idxden) > max(200,0.1*n))
         idxden = [];
      end
   end
%%********************************************************************


%%begin
% We can create a CollisionModel object of the human head and torso by
% running cmdl_hat. This takes a couple of seconds.
hat = cmdl_hat;

% The time is spent performing symbolic arithmetic to build the check
% functions. We can plot the collision model to see how it looks:
hat.plot;

% Let's make it a bit better
campos([1 1 0.5]);
light; lighting gouraud

% We can specify a different shoulder frame of the shoulder for the HAT
% model. The default value is a rotation of -90 deg about the x-axis.
% The indentity matrix will therefore rotate the body to be lying down,
% facing down.
hat2 = cmdl_hat(eye(4));

% Options passed to plot operate on all of the Collision Model's
% primitives
hold on
hat2.plot('FaceAlpha', 0.5);


function model = Learning_WLS_HP(Seqs, model, alg, flag)

D = size(model.A, 1);
L = length(model.landmark);
C = length(Seqs);
XX = [];
YY = [];

for c = 1:C

    Time = Seqs(c).Time;
    if ~isempty(Time)
    Event = Seqs(c).Mark;
    Tstart = Seqs(c).Start;

%     if isempty(alg.Tmax)
%         Tstop = Seqs(c).Stop;
%     else
%         Tstop = alg.Tmax;
%         indt = Time < alg.Tmax;
%         Time = Time(indt);
%         Event = Event(indt);
%     end

    Y = zeros(length(Time), 1);

    if strcmp(flag, 'multisource')
        Xmu = zeros(length(Time), D*C);
    else
        Xmu = zeros(length(Time), D);
    end
    XA = zeros(length(Time), D*L*D);

    for i = 1:length(Time)
        di = Event(i);
        if strcmp(flag, 'multisource')
            Xmu(i, D*(c-1)+di) = 1;
        else
            Xmu(i, di)= 1;
        end

        index = find(Event(1:i)==di);
        Y(i) = length(index);

        if i>1
            dt = Time(i) - Time(1:i-1);
            G = Kernel_Integration(dt, model);

            for j = 1:i-1
                dj = Event(j);
                Gij = G(j,:);
                XA(i, D*L*(di-1)+(dj:L:D*L)) = ...
                    XA(i, D*L*(di-1)+(dj:L:D*L)) + Gij';
            end
        end
    end

    Ytmp = alg.w.*Y./sqrt((Time(:) - Tstart));
    Xtmp = alg.w.*[Xmu, XA]./repmat(sqrt(Time(:) - Tstart), ...
        [1, size(Xmu,2)+size(XA,2)]);
    YY = [YY; Ytmp];
    XX = [XX; Xtmp];
    end
end

res0 = rand(size(XX,2),1);
options = optimoptions('fmincon', 'Algorithm', 'interior-point', ...
    'Display', 'iter', ...
    'MaxFunctionEvaluations', 30000);
res = fmincon(@(x)norm(XX*x - YY)^2, res0, [], [], [], [], ...
    zeros(size(XX,2),1), [], [], options);
%res = XX\YY;
if strcmp(flag, 'multisource')
    model.mu = reshape(res(1:C*D), [D, C]);
    model.A = reshape(res(1+C*D:end), [D, L, D]);
else
    model.mu = reshape(res(1:D), [D, 1]);
    model.A = reshape(res(1+D:end), [D, L, D]);
end


function [faces,vertices] = struct2mesh(mask3M,stlfile,qOffset,voxel_size)
%
% nii = load_untouch_nii(niifile);
% logicalmask = zeros(size(nii.img));
% logicalmask(find(nii.img)) = 1;

if nargin < 4 || ~exist('voxel_size','var') || isempty(voxel_size)
    voxel_size = [1 1 1];
end

if nargin < 3 || ~exist('qOffset','var') || isempty(qOffset)
    sz3M = size(mask3M);
    qOffset = -[voxel_size(1)*sz3M(1) voxel_size(2)*sz3M(2) voxel_size(3)*sz3M(3)]/2;
end

logicalmask = flip(flip(mask3M,1),2);

gridX =voxel_size(1)* [1:size(logicalmask,1)]  + qOffset(1);
gridY = voxel_size(2)*[1:size(logicalmask,2)] + qOffset(2);
gridZ = voxel_size(3)*[1:size(logicalmask,3)] + qOffset(3);

[faces,vertices] = CONVERT_voxels_to_stl(stlfile,logicalmask,gridX,gridY,gridZ,'binary');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT 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.
%
% SPHARM-MAT 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 SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ============================================
% align_cps.m
%
% Goal: Align P to X using ICP
%
% Li Shen
% 07/29/2003 - customized using Hany Farid's icp code.
% 01/08/2004 - customized again for corresponding point set registration.

function [P,M] = align_cps(P,X)

disp('running cps_align');

M = eye(4);

%%%%%%%%%%%%%%%%%%%%
% Added by Li Shen
%%%%%%%%%%%%%%%%%%%%
% center P and X first
[P,cmP] = center(P);
[X,cmX] = center(X);

Xc = X;

muP   = mean(P);
muXc  = mean(Xc);
N     = length(P);
sig   = zeros(3);
for k = 1 : N
  sig = sig + (P(k,:)' * Xc(k,:));
end
sig   = sig/N;
sig   = sig - (muP' * muXc); % cross-covariance matrix
A     = sig - sig';
delta = [A(2,3) ; A(3,1) ; A(1,2)];
Q     = [trace(sig)  delta' ; delta  sig+sig'-trace(sig)*eye(3)];
[V,D] = eig(Q);
[val,ind] = max(diag(D));
qR    = V(:,ind); % optimal rotation
q0    = qR(1);
q1    = qR(2);
q2    = qR(3);
q3    = qR(4);
R     = [q0^2+q1^2-q2^2-q3^2  2*(q1*q2 - q0*q3)  2*(q1*q3 + q0*q2) ; ...
    2*(q1*q2 + q0*q3)  q0^2+q2^2-q1^2-q3^2  2*(q2*q3 - q0*q1) ; ...
    2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1)  q0^2+q3^2-q1^2-q2^2];
qT    = muXc' - R*muP'; % optimal translation

Pnew  = (R*P')';
diff  = mean( sum(((Pnew - P).^2)') );

P = Pnew;

P(:,1) = P(:,1) + qT(1);
P(:,2) = P(:,2) + qT(2);
P(:,3) = P(:,3) + qT(3);

%%%%%%%%%%%%%%%%%%%%
% Added by Li Shen
%%%%%%%%%%%%%%%%%%%%
% recover P and X
P(:,1) = P(:,1) + cmX(1);
P(:,2) = P(:,2) + cmX(2);
P(:,3) = P(:,3) + cmX(3);
X(:,1) = X(:,1) + cmX(1);
X(:,2) = X(:,2) + cmX(2);
X(:,3) = X(:,3) + cmX(3);

Mr = eye(4); Mr(1:3,1:3) = R;
Mt = eye(4); Mt(1:3,4) = qT(1:3)+cmX'-cmP';
M = Mt*Mr*M;

return;

%
% center point set
%

function [P,muP] = center(P)

n = size(P,1);
muP = mean(P);
P = P - muP(ones(1,n),:);

return;


function [est_C_b_e_new,est_v_eb_e_new,est_r_eb_e_new,est_IMU_bias_new,...
            P_matrix_new, corrections, Phi_matrix, Q_matrix] = ...
         LC_KF_Epoch(GNSS_epoch, GNSS_r_eb_e,GNSS_v_eb_e,tor_s, est_C_b_e_old,...
                     est_v_eb_e_old,est_r_eb_e_old,est_IMU_bias_old,...
                     P_matrix_old,meas_f_ib_b,est_L_b_old,LC_KF_config,...
                     Q_matrix, R_matrix, meas_omega_ib_b)
%LC_KF_Epoch - Implements one cycle of the loosely coupled INS/GNSS
% Kalman filter plus closed-loop correction of all inertial states
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 12/4/2012 by Paul Groves
%
% Inputs:
%   GNSS_r_eb_e           GNSS estimated ECEF user position (m)
%   GNSS_v_eb_e           GNSS estimated ECEF user velocity (m/s)
%   tor_s                 propagation interval (s)
%   est_C_b_e_old         prior estimated body to ECEF coordinate
%                         transformation matrix
%   est_v_eb_e_old        prior estimated ECEF user velocity (m/s)
%   est_r_eb_e_old        prior estimated ECEF user position (m)
%   est_IMU_bias_old      prior estimated IMU biases (body axes)
%   P_matrix_old          previous Kalman filter error covariance matrix
%   meas_f_ib_b           measured specific force
%   est_L_b_old           previous latitude solution
%   LC_KF_config
%     .gyro_noise_PSD     Gyro noise PSD (rad^2/s)
%     .accel_noise_PSD    Accelerometer noise PSD (m^2 s^-3)
%     .accel_bias_PSD     Accelerometer bias random walk PSD (m^2 s^-5)
%     .gyro_bias_PSD      Gyro bias random walk PSD (rad^2 s^-3)
%     .pos_meas_SD            Position measurement noise SD per axis (m)
%     .vel_meas_SD            Velocity measurement noise SD per axis (m/s)
%
% Outputs:
%   est_C_b_e_new     updated estimated body to ECEF coordinate
%                      transformation matrix
%   est_v_eb_e_new    updated estimated ECEF user velocity (m/s)
%   est_r_eb_e_new    updated estimated ECEF user position (m)
%   est_IMU_bias_new  updated estimated IMU biases
%     Rows 1-3          estimated accelerometer biases (m/s^2)
%     Rows 4-6          estimated gyro biases (rad/s)
%   P_matrix_new      updated Kalman filter error covariance matrix

% Copyright 2012, Paul Groves
% License: BSD; see GrovesCode/license.txt for details
%
% Edits made by Adam Werries where noted as (Werries), Copyright 2015
% GNSS_epoch
% GNSS_r_eb_e
% GNSS_v_eb_e
% tor_s
% est_C_b_e_old
% est_v_eb_e_old
% est_r_eb_e_old
% est_IMU_bias_old
% P_matrix_old
% meas_f_ib_b
% est_L_b_old
% Q_matrix
% R_matrix
% meas_omega_ib_b
% Constants (sone of these could be changed to inputs at a later date)
c = 299792458; % Speed of light in m/s
omega_ie = 7.292115E-5;  % Earth rotation rate in rad/s
R_0 = 6378137; %WGS84 Equatorial radius in meters
e = 0.0818191908425; %WGS84 eccentricity
Lab = LC_KF_config.lever_arm;
% Begins

% Skew symmetric matrix of Earth rate
Omega_ie = Skew_symmetric([0,0,omega_ie]);

% SYSTEM PROPAGATION PHASE

% 1. Determine transition matrix using (14.50) (first-order approx)
F_21 = -Skew_symmetric(est_C_b_e_old * meas_f_ib_b);
Phi_matrix = eye(15);
Phi_matrix(1:3,1:3) = Phi_matrix(1:3,1:3) - Omega_ie * tor_s;
Phi_matrix(1:3,13:15) = est_C_b_e_old * tor_s;
Phi_matrix(4:6,1:3) = tor_s * F_21;
Phi_matrix(4:6,4:6) = Phi_matrix(4:6,4:6) - 2 * Omega_ie * tor_s;
geocentric_radius = R_0 / sqrt(1 - (e * sin(est_L_b_old))^2) *...
    sqrt(cos(est_L_b_old)^2 + (1 - e^2)^2 * sin(est_L_b_old)^2); % from (2.137)
Phi_matrix(4:6,7:9) = -tor_s * 2 * Gravity_ECEF(est_r_eb_e_old) /...
    geocentric_radius * est_r_eb_e_old' / sqrt (est_r_eb_e_old' *...
    est_r_eb_e_old);
Phi_matrix(4:6,10:12) = est_C_b_e_old * tor_s;
Phi_matrix(7:9,4:6) = eye(3) * tor_s;

% If we were supposed to adapt Q_matrix last iteration, don't replace it.
if GNSS_epoch <= LC_KF_config.n + 1
    % 2. Determine approximate system noise covariance matrix using (14.82)
    Srg = LC_KF_config.gyro_noise_PSD;
    Sra = LC_KF_config.accel_noise_PSD;
    Sbad = LC_KF_config.accel_bias_PSD;
    Sbgd = LC_KF_config.gyro_bias_PSD;
    Q11 = eye(3) * (Srg * tor_s + Sbgd*tor_s^3/3);
    Q21 = (Srg*tor_s^2 / 2 + Sbgd*tor_s^4 / 4) * F_21;
    Q31 = (Srg*tor_s^3 / 3 + Sbgd*tor_s^5 / 5) * F_21;
    Q15 = Sbgd*tor_s^2*est_C_b_e_old / 2;
    Q22 = (Sra*tor_s + Sbad*tor_s^3/3)*eye(3) + (Srg*tor_s^3/3 + Sbgd*tor_s^5/5)*(F_21*F_21');
    Q32 = (Sra*tor_s^2/2 + Sbad*tor_s^4/4)*eye(3) + (Srg*tor_s^4/4 + Sbgd*tor_s^6/6)*(F_21*F_21');
    Q24 = Sbad*tor_s^2*est_C_b_e_old / 2;
    Q25 = Sbgd*tor_s^3*F_21*est_C_b_e_old / 3;
    Q33 = (Sra*tor_s^3/3 + Sbad*tor_s^5/5)*eye(3) + (Srg*tor_s^5/5 + Sbgd*tor_s^7/7)*(F_21*F_21');
    Q34 = Sbad*tor_s^3*est_C_b_e_old/3;
    Q35 = Sbgd*tor_s^4*F_21*est_C_b_e_old/4;
    Q44 = Sbad*tor_s*eye(3);
    Q55 = Sbgd*tor_s*eye(3);
    Q_matrix = [Q11      Q21' Q31' zeros(3) Q15;
                Q21      Q22  Q32' Q24      Q25;
                Q31      Q32  Q33  Q34      Q35;
                zeros(3) Q24' Q34' Q44      zeros(3);
                Q15'     Q25' Q35' zeros(3) Q55;];
else
    Q_matrix = tor_s*Q_matrix;
end
[~,p] = chol(Q_matrix);
if p ~= 0
    Q_matrix = nearestSPD(Q_matrix);
end
% 3. Propagate state estimates using (3.14) noting that all states are zero
% due to closed-loop correction.
x_est_propagated(1:15,1) = 0;

% 4. Propagate state estimation error covariance matrix using (3.46)
P_matrix_propagated = Phi_matrix * P_matrix_old * Phi_matrix' + Q_matrix;

% MEASUREMENT UPDATE PHASE
% 5. Set-up measurement matrix using (14.115)
H_matrix = zeros(6,15);
H_matrix(1:3,1:3) = Skew_symmetric(est_C_b_e_old*Lab);
H_matrix(4:6,1:3) = Skew_symmetric(est_C_b_e_old*cross(meas_omega_ib_b,Lab) ...
                                   - Omega_ie*est_C_b_e_old*Lab);
H_matrix(1:3,7:9) = -eye(3);
H_matrix(4:6,4:6) = -eye(3);
H_matrix(4:6,13:15) = est_C_b_e_old*Skew_symmetric(Lab);

% 6. Set-up measurement noise covariance matrix assuming all components of
% GNSS position and velocity are independent and have equal variance.

% 7. Calculate Kalman gain using (3.21)
K_matrix = P_matrix_propagated * H_matrix' / (H_matrix * P_matrix_propagated * H_matrix' + R_matrix);

% 8. Formulate measurement innovations using (14.102)
% Lever arm from INS to GPS
delta_z(1:3,1) = GNSS_r_eb_e - est_r_eb_e_old - est_C_b_e_old*Lab - est_C_b_e_old*LC_KF_config.gps_correction;
delta_z(4:6,1) = GNSS_v_eb_e - est_v_eb_e_old ...
                  - est_C_b_e_old*cross(meas_omega_ib_b,Lab) ...
                  + Omega_ie*est_C_b_e_old*Lab;

% 9. Update state estimates using (3.24)
x_est_new = x_est_propagated + K_matrix * delta_z;
corrections = x_est_new;

% 10. Update state estimation error covariance matrix using (3.25)
P_matrix_new = (eye(15) - K_matrix * H_matrix) * P_matrix_propagated;
[~,p] = chol(P_matrix_new);
if p ~= 0
    P_matrix_new = nearestSPD(P_matrix_new);
end
% CLOSED-LOOP CORRECTION

% Correct attitude, velocity, and position using (14.7-9)
est_C_b_e_new = (eye(3) - Skew_symmetric(x_est_new(1:3))) * est_C_b_e_old;
est_v_eb_e_new = est_v_eb_e_old - x_est_new(4:6);
est_r_eb_e_new = est_r_eb_e_old - x_est_new(7:9);

% Return residual error
% residual = [GNSS_r_eb_e; GNSS_v_eb_e] - [est_r_eb_e_new; est_v_eb_e_new];
% residual = [est_r_eb_e_old; est_v_eb_e_old] - [est_r_eb_e_new; est_v_eb_e_new];
% Update IMU bias estimates
est_IMU_bias_new = est_IMU_bias_old + x_est_new(10:15);

% Ends


% Simple script to read a video file and track a single red marker
% in 2D using the hue and saturation values. The largest red blob is
% detected and the mean co-ordinate position of this blob is tracked. This
% code does not have any tracking built-in: it treats each frame
% idependently.
%
% This assumes that the video is a constant frame rate video. The resulting
% co-ordinate time series are stored in the variables: t, Xc and Yc.

fname = 'Capture 1 (5-10-2011 3-29 PM).wmv';
% Download from http://www.youtube.com/watch?v=Thnv0IQqXFE
% for a sample video on which this program was tested.

hmfr = video.MultimediaFileReader(fname);
hp  = video.VideoPlayer;
hpI = video.VideoPlayer;

hcsc = video.ColorSpaceConverter;
hcsc.Conversion = 'RGB to HSV';

fi = 0;
fps = 30;
refColor = [0.8 0.8];
cD = 0.05;
start_t = 0;
Xc = [];
Yc = [];
t  = [];
while ~isDone(hmfr)
    fi = fi + 1;
    tf = fi/fps;
    videoFrame = step(hmfr);
    if tf < start_t
        continue;
    end
    vI =  step(hcsc, videoFrame);
    [xc, yc, vI] = FrameByColorDistance(vI, refColor, cD);
    Xc = [Xc; xc]; %#ok<AGROW>
    Yc = [Yc; yc]; %#ok<AGROW>
    t  = [t; tf];  %#ok<AGROW>
    DisplayFrame(hp, videoFrame, fi, fps);
    DisplayFrame(hpI, floor(vI*255), fi, fps);
end
release(hp);
release(hmfr);


function smallpot = marginalize_pot(bigpot, keep, maximize, useC)
% MARGINALIZE_POT Marginalize a cgpot onto a smaller domain.
% smallpot = marginalize_pot(bigpot, keep, maximize, useC)
%
% If maximize = 1, we raise an error.
% useC is ignored.

if nargin < 3, maximize = 0; end
assert(~maximize);

sumover = mysetdiff(bigpot.domain, keep);
csumover = myintersect(sumover, bigpot.cdom);
dsumover = myintersect(sumover, bigpot.ddom);
dkeep = myintersect(keep, bigpot.ddom);
ckeep = myintersect(keep, bigpot.cdom);
%ns = sparse(1, max(bigpot.domain)); % must be full, so I is an integer
ns = zeros(1, max(bigpot.domain));
ns(bigpot.ddom) = bigpot.dsizes;
ns(bigpot.cdom) = bigpot.csizes;

% sum(ns(csumover))==0 is like isempty(csumover) but handles observed nodes.
% Similarly, prod(ns(dsumover))==1 is like isempty(dsumover)

% Marginalize the cts parts.
% If we are in canonical form, we stay that way, since moment form might not exist.
% Besides, we would like to minimize the number of conversions.
if sum(ns(csumover)) > 0
  if bigpot.subtype == 'm'
    for i=1:bigpot.dsize
      bigpot.mom{i} = marginalize_pot(bigpot.mom{i}, ckeep);
    end
  else
    for i=1:bigpot.dsize
      bigpot.can{i} = marginalize_pot(bigpot.can{i}, ckeep);
    end
  end
end

% If we are not marginalizing over any discrete nodes, we are done.
if prod(ns(dsumover))==1
  smallpot = cgpot(dkeep, ckeep, ns, bigpot.can, bigpot.mom, bigpot.subtype);
  return;
end

% To marginalize the discrete parts, we partition the cts parts into those that depend
% on dkeep (i) and those that depend on on dsumover (j).

I = prod(ns(dkeep));
J = prod(ns(dsumover));
C = sum(ns(ckeep));
sum_map = find_equiv_posns(dsumover, bigpot.ddom);
keep_map = find_equiv_posns(dkeep, bigpot.ddom);
iv = zeros(1, length(bigpot.ddom)); % index vector

% If in canonical form, marginalize if possible, else convert to moment form.
if 0 & bigpot.subtype == 'c'
  p1 = zeros(I,J);
  h1 = zeros(C,J,I);
  K1 = zeros(C,C,J,I);
  for i=1:I
    keep_iv = ind2subv(ns(dkeep), i);
    iv(keep_map) = keep_iv;
    for j=1:J
      sum_iv = ind2subv(ns(dsumover), j);
      iv(sum_map) = sum_iv;
      k = subv2ind(ns(bigpot.ddom), iv);
      can = struct(bigpot.can{k}); % violate object privacy
      p1(i,j) = exp(can.g);
      if C > 0 % so mu1 and Sigma1 are non-empty
	h1(:,j,i) = can.h;
	K1(:,:,j,i) = can.K;
      end
    end
  end

  % If the cts parts do not depend on j, we can just marginalize the weighting coefficient g.
  jdepends = 0;
  for i=1:I
    for j=2:J
      if ~approxeq(h1(:,j,i), h1(:,1,i)) | ~approxeq(K1(:,:,j,i), K1(:,:,1,i))
	jdepends = 1;
	break
      end
    end
  end

  if ~jdepends
    %g2 = log(sum(p1, 2));
    g2 = zeros(I,1);
    for i=1:I
      s = sum(p1(i,:));
      if s > 0
	g2(i) = log(s);
      end
    end
    h2 = h1;
    K2 = K1;
    can = cell(1,I);
    j = 1; % arbitrary
    for i=1:I
      can{i} = cpot(ckeep, ns(ckeep), g2(i), h2(:,j,i), K2(:,:,j,i));
    end
    smallpot = cgpot(dkeep, ckeep, ns, can, [], 'c');
    return;
  else
    % Since the cts parts depend on j, we must convert to moment form
    bigpot = cg_can_to_mom(bigpot);
  end
end

% Marginalize in moment form
bigpot = cg_can_to_mom(bigpot);

% Now partition the moment components.
T1 = zeros(I,J);
mu1 = zeros(C,J,I);
Sigma1 = zeros(C,C,J,I);
for i=1:I
  keep_iv = ind2subv(ns(dkeep), i);
  iv(keep_map) = keep_iv;
  for j=1:J
    sum_iv = ind2subv(ns(dsumover), j);
    iv(sum_map) = sum_iv;
    k = subv2ind(ns(bigpot.ddom), iv);
    mom = struct(bigpot.mom{k}); % violate object privacy
    T1(i,j) = exp(mom.logp);
    if C > 0 % so mu1 and Sigma1 are non-empty
      mu1(:,j,i) = mom.mu;
      Sigma1(:,:,j,i) = mom.Sigma;
    end
  end
end

% Collapse the mixture of Gaussians
coef = mk_stochastic(T1); % coef must be convex combination
T2 = sum(T1,2);
T2 = T2 + (T2==0)*eps;
%if C > 0, disp('collapsing onto '); disp(leep); end
mu = [];
Sigma = [];
mom = cell(1,I);
for i=1:I
  if C > 0
    [mu, Sigma] = collapse_mog(mu1(:,:,i), Sigma1(:,:,:,i), coef(i,:));
  end
  logp = log(T2(i));
  mom{i} = mpot(ckeep, ns(ckeep), logp, mu, Sigma);
end

smallpot = cgpot(dkeep, ckeep, ns, [], mom, 'm');


%% driver for statistical and systems heterogeneity experiments

%% load dataset
datarepo = 'data/'; % location of data folder
dataset = 'small'; % small test dataset
load([datarepo dataset]); % load data

%% set parameters
addpath('opt/'); addpath('util/'); % add helper functions
training_percent = 0.75; % percentage of data for training
[Xtrain, Ytrain, Xtest, Ytest] = split_data(X, Y, training_percent);
opts.obj='C'; % classification
opts.avg = 1; % compute average error
opts.sys_het = 0; % run systems (1) or stats heterogeneity exps (0)
opts.top = 1.0; % highest number of rounds
opts.bottom = 0.1; % lowest number of rounds
lambda = 1e-4;

%% mocha [need to tune sdca_frac]
opts.mocha_outer_iters = 1;
opts.mocha_inner_iters = 2000;
opts.mocha_sdca_frac = 1.0;
opts.w_update = 1; % just do a single w-update
[rmse_mocha, primal_mocha, dual_mocha] = run_mocha(Xtrain, Ytrain, Xtest, Ytest, lambda, opts);

%% cocoa [need to tune sdca_frac]
opts.cocoa_outer_iters = 1;
opts.cocoa_inner_iters = 500;
opts.theta = 0.5;
[rmse_cocoa, primal_cocoa, dual_cocoa, max_its] = run_cocoa(Xtrain, Ytrain, Xtest, Ytest, lambda, opts);

%% mbsdca [need to tune sdca_frac, scaling]
opts.mbsdca_outer_iters = 1;
opts.mbsdca_inner_iters = 5000;
opts.mbsdca_sdca_frac = 0.5;
opts.mbsdca_scaling = 10;
[rmse_mbsdca, primal_mbsdca, dual_mbsdca] = run_mbsdca(Xtrain, Ytrain, Xtest, Ytest, lambda, opts);

%% mbsgd [need to tune sgd_frac, scaling]
opts.mbsgd_outer_iters = 1;
opts.mbsgd_inner_iters = 5000;
opts.mbsgd_sgd_frac = 0.5;
opts.mbsgd_scaling = 0.1;
[rmse_mbsgd, primal_mbsgd] = run_mbsgd(Xtrain, Ytrain, Xtest, Ytest, lambda, opts);

if(opts.sys_het)
    %% plot systems heterogeneity
    % note: ensure methods have reached global optimal or enter manually
    optimal = min([primal_mocha; primal_cocoa; primal_mbsdca; primal_mbsgd]);

    %% calculate estimated time
    comm_cost = 100; % communication cost: Wifi=10, LTE=100, 3G=1000
    train_n = 0;
    for t=1:length(Xtrain)
         train_n = train_n + size(Xtrain{t},1);
    end

    %% calculate time based on flops and communication cost
    local_mocha_time = 8 * opts.top * train_n + comm_cost;
    local_mbsdca_time = 6 * opts.top * train_n + comm_cost;
    local_mbsgd_time = 4 * opts.top * train_n + comm_cost;
    mocha_time = 1:local_mocha_time:local_mocha_time*length(primal_mocha);
    cocoa_time = cumsum(max_its .* (8 * length(Xtrain)) + comm_cost);
    mbsdca_time = 1:local_mbsdca_time:local_mbsdca_time*length(primal_mbsdca);
    mbsgd_time = 1:local_mbsgd_time:local_mbsgd_time*length(primal_mbsgd);

    %% plot results
    figure;
    step = 100;
    semilogy(mocha_time(1:step:end), primal_mocha(1:step:end) - optimal, 'LineWidth', 6)
    hold on;
    semilogy(cocoa_time(1:step:end), primal_cocoa(1:step:end) - optimal, 'LineWidth', 6)
    hold on;
    semilogy(mbsdca_time(1:step:end), primal_mbsdca(1:step:end) - optimal, 'LineWidth', 6)
    hold on;
    semilogy(mbsgd_time(1:step:end), primal_mbsgd(1:step:end) - optimal, 'LineWidth', 6)
    title([dataset ': Systems Heterogeneity'])
    xlabel('Estimated Time')
    ylabel('Primal Sub-Optimality')
    set(gca, 'fontsize', 16)
    legend({'MOCHA', 'CoCoA', 'Mb-SDCA', 'Mb-SGD'})
    axis([0 8000000 .001 100]) % set manually

else
    %% plot statistical heterogeneity
    % note: ensure methods have reached global optimal or enter manually
    optimal = min([primal_mocha; primal_cocoa; primal_mbsdca; primal_mbsgd]);

    %% calculate time based on flops and communication cost
    train_n = 0;
    for t=1:length(Xtrain)
         train_n = train_n + size(Xtrain{t},1);
    end
    comm_cost = 100; % communication cost: Wifi=10, LTE=100, 3G=1000
    local_mocha_time = 8 * opts.mocha_sdca_frac * train_n + comm_cost;
    local_mbsdca_time = 6 * opts.mbsdca_sdca_frac * train_n + comm_cost;
    local_mbsgd_time = 4 * opts.mbsgd_sgd_frac * train_n + comm_cost;
    cocoa_time = cumsum(max_its .* (8 * length(Xtrain)) + comm_cost);
    mocha_time = 1:local_mocha_time:local_mocha_time*length(primal_mocha);
    mbsdca_time = 1:local_mbsdca_time:local_mbsdca_time*length(primal_mbsdca);
    mbsgd_time = 1:local_mbsgd_time:local_mbsgd_time*length(primal_mbsgd);

    %% plot results
    figure;
    step = 100;
    semilogy(mocha_time(1:step:end), primal_mocha(1:step:end) - optimal, 'LineWidth', 6)
    hold on;
    semilogy(cocoa_time, primal_cocoa - optimal, 'LineWidth', 6)
    hold on;
    semilogy(mbsdca_time(1:step:end), primal_mbsdca(1:step:end) - optimal, 'LineWidth', 6)
    hold on;
    semilogy(mbsgd_time(1:step:end), primal_mbsgd(1:step:end) - optimal, 'LineWidth', 6)
    title([dataset ': Statistical Heterogeneity'])
    xlabel('Estimated Time')
    ylabel('Primal Sub-Optimality')
    set(gca, 'fontsize', 16)
    legend({'MOCHA', 'CoCoA', 'Mb-SDCA', 'Mb-SGD'})
    axis([0 7000000 .001 100]) % set manually

end