% ------------------------------------------------------------------------
%  Copyright (C)
%  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
%  University of California Berkeley (UCB) - USA
%
%  Jordi Pont-Tuset <jordi.pont@upc.edu>
%  Pablo Arbelaez <arbelaez@berkeley.edu>
%  June 2014
% ------------------------------------------------------------------------
% This file is part of the MCG package presented in:
%    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
%    "Multiscale Combinatorial Grouping,"
%    Computer Vision and Pattern Recognition (CVPR) 2014.
% Please consider citing the paper if you use this code.
% ------------------------------------------------------------------------
function rank_training(params, cache_dir)

n_hiers = length(params.hiers);

%% Load Pareto parameters
n_cands = loadvar(params.files.pareto_point,'n_cands');

% Some checks
assert(n_hiers==size(n_cands,2));
assert(params.n_r_cand==size(n_cands,1));

%% Gather all features and quality of the training set candidates
if exist(params.files.features_file,'file')
    load(params.files.features_file);
    disp(['Loaded: ' params.files.features_file '.'])
else
    disp(['RECOMPUTING: ' params.files.features_file '.'])

    % features = [];
    % jaccards = [];

    % Load which images to consider from the params.database (train, val, etc.)
    im_ids = database_ids(params.database,params.gt_set_ranking);

    num_images = length(im_ids);
    parfor im_id = 1:num_images
        out = cache_mcg_features(params, [], im_ids{im_id}, cache_dir);
        f_lp = out.f_lp; f_ms = full(out.f_ms); feats = out.feats;
        bboxes = out.bboxes; red_cands = out.red_cands;
        b_feats_intersections = full(out.b_feats_intersection);

        if(params.depth_features)
          tt = tic();
          sp = f_lp;
          sp2regC = cands2labels(red_cands, f_ms);
          sp2reg = false(max(sp(:)), length(sp2regC));
          for i = 1:length(sp2regC), sp2reg(sp2regC{i},i) = true; end
          D = getImage(im_ids{im_id}, 'depth');
          D = double(D)./1000;
          C = params.camera_matrix;
          missingMask = getImage(im_ids{im_id}, 'rawdepth') == 0;
          fdepth = depthFeatures(sp, sp2reg, D, missingMask, C);
          feats = cat(2, feats, fdepth');
          fprintf('%s: Time for depth features: %0.3f\n', im_ids{im_id}, toc(tt));
        end

        % Eval candidates
        gt = get_ground_truth(params.database, im_ids{im_id});
        hier = struct('leaves_part', f_lp, 'ms_struct', ms_matrix2struct(f_ms));
        jacc = eval_cands(hier, red_cands, gt);
        max_jacc = max(jacc,[],1)';

        % fprintf('.');
        % Sample candidates
        % Get the optimum candidates for each object
        % to ensure they are on the training set
        [~,ids] = max(jacc,[],2);
        if (length(ids)>params.n_samples)  % More objects than samples asked
            ids = ids(1:params.n_samples);
        end
        ids_rest = setdiff(1:size(jacc,2),ids);
        if (size(jacc,2)-length(ids))>params.n_samples
            ids_rest = ids_rest(randperm(length(ids_rest),params.n_samples-length(ids)));
        end
        if isrow(ids_rest)
            ids_rest = ids_rest';
        end
        sel_ids = [ids; ids_rest];

        % Store
        % features = [features; feats(sel_ids,:)]; %#ok<AGROW>
        % jaccards = [jaccards; max_jacc(sel_ids,:)]; %#ok<AGROW>

        features{im_id} = feats(sel_ids,:);
        jaccards{im_id} = max_jacc(sel_ids,:);
    end
    features = cat(1, features{:});
    jaccards = cat(1, jaccards{:});

    save(params.files.features_file,'features','jaccards');
end

%% Train the random forest
if exist(params.files.trained_classifier,'file')
    disp(['Already trained: ' params.files.trained_classifier '.'])
else
    disp(['TRAINING: ' params.files.trained_classifier '.'])

    % Train and save the result
    rf = regRF_train(features,jaccards,50); %#ok<NASGU>
    disp('Training done')

    % Save the result
    save(params.files.trained_classifier,'rf');
end


function Ml = hmxPlus(Ml,Mr)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxPlus.m                                     |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Sum of H-Matrix with the rule                 |
%|  `---'  |                Full > H-Matrix > Compr                       |
%+========================================================================+

% Check dimensions
if sum(size(Ml) ~= size(Mr))
    error('hmxPlus.m : matrix dimensions must agree.')
end

%%% H-Matrix + H-Matrix -> H-Matrix
if isa(Ml,'hmx') && isa(Mr,'hmx')
    % H-Matrix + H-Matrix --> H-Matrix (recursion)
    if (Ml.typ == 0) && (Mr.typ == 0)
        % Construction
        for i = 1:4
            Ml.chd{i} = hmxPlus(Ml.chd{i},Mr.chd{i});
        end

        % Fusion
        Ml = hmxFusion(Ml);

    % H-Matrix + Compr -> H-Matrix
    elseif (Ml.typ == 0) && (Mr.typ == 1)
        Ml = hmxPlusAB(Ml,Mr.dat{1},Mr.dat{2});

    % H-Matrix + Full -> Unavailable
    elseif (Ml.typ == 0) && (Mr.typ == 2)
        error('hmxPlus : unvailable case')

    % Compr + --- -> ---
    elseif (Ml.typ == 1)
        Ml = hmxPlusAB(Mr,Ml.dat{1},Ml.dat{2});

    % Full + H-Matrix -> Unavailable
    elseif (Ml.typ == 2) && (Mr.typ == 0)
        error('hmxPlus : unvailable case')

    % Full + Compr -> Full
    elseif (Ml.typ == 2) && (Mr.typ == 1)
        Ml = hmxPlusAB(Ml,Mr.dat{1},Mr.dat{2});

    % Full + Full -> Full
    elseif (Ml.typ == 2) && (Mr.typ == 2)
        Ml.dat = Ml.dat + Mr.dat;

    else
        error('hmxPlus : unvailable case')
    end

%%% H-Matrix + Matrix -> H-Matrix
elseif isa(Ml,'hmx')
    Ml = Ml + hmx(Ml.pos{1},Ml.pos{2},Mr,Ml.tol);

%%% Matrix + H-Matrix -> Matrix
elseif isa(Mr,'hmx')
    Ml = hmx(Mr.pos{1},Mr.pos{2},Ml,Mr.tol) + Mr;

%%% Unavailable
else
    error('hmxPlus.m : unavailable case')
end
end


function voi = bf_sources_voi(BF, S)
% Generate a set of VOIs specified in MNI coordinates
% Copyright (C) 2013 Wellcome Trust Centre for Neuroimaging

% $Id: bf_sources_voi.m 7703 2019-11-22 12:06:29Z guillaume $

%--------------------------------------------------------------------------
if nargin == 0
    label = cfg_entry;
    label.tag = 'label';
    label.name = 'Label';
    label.strtype = 's';
    label.help = {'Label for the VOI'};

    pos = cfg_entry;
    pos.tag = 'pos';
    pos.name = 'MNI coordinates';
    pos.strtype = 'r';
    pos.num = [1 3];
    pos.help = {'Locations for the VOI in MNI coordinates'};
    pos.val = {};

    ori = cfg_entry;
    ori.tag = 'ori';
    ori.name = 'Orientation';
    ori.strtype = 'r';
    ori.num = [1 3];
    ori.help = {'Source orientatons (only for single points, leave zeros for unoriented)'};
    ori.val = {[0 0 0]};

    voidef = cfg_branch;
    voidef.tag = 'voidef';
    voidef.name = 'VOI';
    voidef.val = {label, pos, ori};

    mask = cfg_files;
    mask.tag = 'mask';
    mask.name = 'MNI mask';
    mask.filter = 'image';
    mask.ufilter = '.*';
    mask.num     = [1 1];
    mask.help = {'Select a mask image'};

    maskdef = cfg_branch;
    maskdef.tag = 'maskdef';
    maskdef.name = 'Mask VOI';
    maskdef.val  = {label, mask};

    vois = cfg_repeat;
    vois.tag = 'vois';
    vois.name = 'VOIs';
    vois.num  = [1 Inf];
    vois.values = {voidef, maskdef};
    vois.val = {voidef};

    radius = cfg_entry;
    radius.tag = 'radius';
    radius.name = 'Radius';
    radius.strtype = 'r';
    radius.num = [1 1];
    radius.val = {0};
    radius.help = {'Radius (in mm) for the VOIs (leave 0 for single point)'};

    resolution = cfg_entry;
    resolution.tag = 'resolution';
    resolution.name = 'Resolution';
    resolution.strtype = 'r';
    resolution.num = [1 1];
    resolution.val = {5};
    resolution.help = {'Resolution for placing grid points in each VOI (in mm)'};

    voi = cfg_branch;
    voi.tag = 'voi';
    voi.name = 'VOIs in MNI space';
    voi.val = {vois, radius, resolution};

    return
elseif nargin < 2
    error('Two input arguments are required');
end

iskull = export(gifti(BF.data.mesh.tess_iskull), 'ft');

M1 = BF.data.transforms.toNative;
M1 = BF.data.transforms.toMNI/M1;

iskull = ft_convert_units(ft_transform_geometry(M1, iskull));

% transform MNI coords in MNI space into space where we are doing the
% beamforming
M = inv(BF.data.transforms.toMNI);

if S.radius > 0
    vec = -S.radius:S.resolution:S.radius;
    [X, Y, Z]  = ndgrid(vec, vec, vec);
    sphere   = [X(:) Y(:) Z(:)];
    sphere(sqrt(X(:).^2 + Y(:).^2 + Z(:).^2) > S.radius, :) = [];
    npnt = size(sphere, 1);
else
    sphere = 0;
    npnt = 1;
end

grid = bf_sources_grid(BF, struct('resolution', S.resolution, 'space', 'MNI template'));
mnigrid = ft_transform_geometry(BF.data.transforms.toMNI, grid);

nvoi = numel(S.vois);
voi = [];
voi.label = {};
voi.pos = [];
ori = [];
voi.pos2voi = [];

for i = 1:nvoi
    switch char(fieldnames(S.vois{i}))
        case 'voidef'
            voi.label{i} = S.vois{i}.voidef.label;
            voi.pos = [voi.pos; sphere+repmat(S.vois{i}.voidef.pos, npnt, 1)];
            ori     = [ori; S.vois{i}.voidef.ori];
            voi.pos2voi  = [voi.pos2voi i*ones(1, npnt)];
        case 'maskdef'
            voi.label{i} = S.vois{i}.maskdef.label;
            V   = spm_vol(char(S.vois{i}.maskdef.mask));

            vox = spm_eeg_inv_transform_points(inv(V.mat), mnigrid.pos);
            Y   = spm_sample_vol(V, vox(:, 1),  vox(:, 2), vox(:, 3), 0);
            ind = find(~isnan(Y) & abs(Y)>0);
            voi.pos = [voi.pos; mnigrid.pos(ind, :)];
            ori = [ori;zeros(length(ind), 3)];
            voi.pos2voi  = [voi.pos2voi i*ones(1, length(ind))];
    end
end

voi.label = voi.label(:);

% Remove points outside the brain
inside = ft_inside_headmodel(voi.pos, struct('bnd', iskull));

voi.pos(~inside, :)  = [];
voi.pos2voi(~inside) = [];

if any(any(ori))
    voi.ori = ori(inside, :);
end

voi = ft_transform_geometry(M, voi);


function [Y, xY] = spm_summarise(V,xY,fhandle,keepNaNs)
% Summarise data within a Region of Interest
% FORMAT [Y, xY] = spm_summarise(V,xY,fhandle)
% V       - [1 x n] vector of mapped image volumes to read (from spm_vol)
%           Or a char array of filenames
% xY      - VOI structure (from spm_ROI)
%           Or a VOI_*.mat (from spm_regions) or a mask image filename
%           Or the keyword 'all' to summarise all voxels in the images
%           Or a [3 x m] matrix of voxel coordinates {mm}
% fhandle - function handle to be applied on image data within VOI
%           Must transform a [1 x m] array into a [1 x p] array
%           Default is Identity (returns raw data, vectorised into rows).
%           Can also use keyword 'litres' to compute the total volume,
%           within the region of interest, for a tissue segment image.
%
% Y       - [n x p] data summary
% xY      - (updated) VOI structure
%__________________________________________________________________________
%
% Example:
% spm_summarise('beta_0001.nii',...
%               struct('def','sphere', 'spec',8, 'xyz',[10 20 30]'),...
%               @mean)
%__________________________________________________________________________
% Copyright (C) 2010-2015 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin, Ged Ridgway
% $Id: spm_summarise.m 7384 2018-07-31 13:36:15Z guillaume $

%-Argument checks
%--------------------------------------------------------------------------
if nargin < 1 || isempty(V)
    [V, sts] = spm_select([1 Inf], 'image', 'Specify Images');
    if ~sts, error('Must select 1 or more images'), end
end
if iscellstr(V), V = char(V); end
if ischar(V), V = spm_data_hdr_read(V); end
spm_check_orientations(V);

if nargin < 2 || isempty(xY), xY = struct; end
if ischar(xY)
    if strcmpi(xY, 'all')
        xY = struct('def', 'all');
    elseif any(regexpi(xY, '\.mat$'))
        try
            load(xY,'xY'); % VOI_*.mat file. Warns if .mat has no xY ...
            xY = rmfield(xY,'XYZmm'); % ...  error if .mat has no xY
        catch
            xY = struct; % GUI specification in spm_ROI
        end
    else % assume mask image filename
        xY = struct('def','mask', 'spec',xY);
    end
elseif isnumeric(xY) && any(size(xY, 1) == [3 4])
    xY = struct('XYZmm', xY(1:3, :));
elseif isstruct(xY) && isfield(xY,'fname')
    xY = struct('def','mask', 'spec',xY);
elseif ~isstruct(xY)
    error('Incorrect xY specified')
end
if ~isfield(xY,'XYZmm'), [xY, xY.XYZmm] = spm_ROI(xY,V(1)); end

if nargin < 3 || isempty(fhandle), fhandle = @(x) x; end
if ischar(fhandle) && strcmp(fhandle, 'litres')
    vsz     = abs(det(V(1).mat));  % voxel size in mm^3
    fhandle = @(x) sum(x) * vsz / 1e6;
end
if ischar(fhandle), fhandle = str2func(fhandle); end

% Undocumented option in case anyone wants to keep (e.g. to check for) NaNs
if nargin < 4, keepNaNs = false; end
if keepNaNs
    dropNaNs = @(x) x;
else
    dropNaNs = @(x) x(~isnan(x));
end

%-Summarise data
%--------------------------------------------------------------------------
XYZ = round(V(1).mat \ [xY.XYZmm; ones(1, size(xY.XYZmm, 2))]);

% Run on first volume to determine p, and transpose if column vector
Y = fhandle(dropNaNs(spm_data_read(V(1), 'xyz', XYZ)));
if ndims(Y) > 2
    error('Function must return a [1 x p] array')
elseif size(Y, 1) ~= 1
    if size(Y, 2) == 1
        Y = Y';
    else
        error('Function returned a [%d x %d] array instead of [1 x p]', ...
            size(Y, 1), size(Y, 2))
    end
end

% Preallocate space and then run on remaining volumes
Y(2:numel(V), :) = 0;
for i = 2:numel(V)
    Y(i, :) = fhandle(dropNaNs(spm_data_read(V(i), 'xyz', XYZ)));
end


function mesh = prepare_mesh_headshape(cfg)

% PREPARE_MESH_HEADSHAPE
%
% Configuration options:
%   cfg.headshape   = a filename containing headshape, a Nx3 matrix with surface
%                     points, or a structure with a single or multiple boundaries
%   cfg.smooth      = a scalar indicating the number of non-shrinking
%                     smoothing iterations (default = no smoothing)
%
% See also PREPARE_MESH_MANUAL, PREPARE_MESH_SEGMENTATION

% Copyrights (C) 2009, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% get the specific options
cfg.headshape    = ft_getopt(cfg, 'headshape');
cfg.smooth       = ft_getopt(cfg, 'smooth');   % no default

if isa(cfg, 'config')
  % convert the config-object back into a normal structure
  cfg = struct(cfg);
end

if isa(cfg.headshape, 'config')
  % convert the nested config-object back into a normal structure
  cfg.headshape = struct(cfg.headshape);
end

% get the surface describing the head shape
if isstruct(cfg.headshape) && numel(cfg.headshape)>1
  % this applies for multilayer BEM models and concentric sphere models
  headshape = [];
  for i=1:numel(cfg.headshape)
    [headshape(i).pos, headshape(i).tri] = headsurface([], [], 'headshape', cfg.headshape(i));
  end
else
  [headshape.pos, headshape.tri] = headsurface([], [], 'headshape', cfg.headshape);
end

if numel(headshape)>1 && numel(cfg.numvertices)==1
  % use the same number of vertices for each of the head shape surfaces
  cfg.numvertices = repmat(cfg.numvertices, size(headshape));
end

if ~isempty(cfg.numvertices) && ~isequal(cfg.numvertices, arrayfun(@(x) size(x.pos, 1), headshape))
  for i=1:numel(headshape)
    tri1 = headshape(i).tri;
    pos1 = headshape(i).pos;
    % The number of vertices is multiplied by 3 in order to have more
    % points on the original mesh than on the sphere mesh (see below).
    % The rationale for this is that every projection point on the sphere
    % has three corresponding points on the mesh
    if (cfg.numvertices(i)>size(pos1,1))
      [tri1, pos1] = refinepatch(headshape(i).tri, headshape(i).pos, 3*cfg.numvertices(i));
    else
      [tri1, pos1] = reducepatch(headshape(i).tri, headshape(i).pos, 3*cfg.numvertices(i));
    end

    % remove double vertices
    [pos1, tri1] = remove_double_vertices(pos1, tri1);

    % replace the probably unevenly distributed triangulation with a regular one
    % and retriangulate it to the desired accuracy
    [pos2, tri2] = mysphere(cfg.numvertices(i)); % this is a regular triangulation
    [pos1, tri1] = retriangulate(pos1, tri1, pos2, tri2, 2);
    [pos1, tri1] = fairsurface(pos1, tri1, 1); % this helps redistribute the superimposed points

    % remove double vertices
    [headshape(i).pos, headshape(i).tri] = remove_double_vertices(pos1, tri1);
    fprintf('returning %d vertices, %d triangles\n', size(headshape(i).pos,1), size(headshape(i).tri,1));
  end
end

% smooth the mesh
if ~isempty(cfg.smooth)
  for i=1:numel(headshape)
    [headshape(i).pos,headshape(i).tri] = fairsurface(headshape(i).pos, headshape(i).tri, cfg.smooth);
  end
end

% the output should only describe one or multiple boundaries and should not
% include any other fields
mesh = keepfields(headshape, {'pos', 'tri'});

function [tri1, pos1] = refinepatch(tri, pos, numvertices)
fprintf('the original mesh has %d vertices, the requested number of vertices is %d\n',size(pos,1),numvertices/3);
fprintf('trying to refine the mesh...\n');
[pos1, tri1] = refine(pos, tri, 'updown', numvertices);

function [pos, tri] = mysphere(N)
% This is a copy of MSPHERE without the confusing output message
% Returns a triangulated sphere with approximately M vertices
% that are nicely distributed over the sphere. The vertices are aligned
% along equally spaced horizontal contours according to an algorithm of
% Dave Russel.
%
% Use as
%  [pos, tri] = msphere(M)
%
% See also SPHERE, NSPHERE, ICOSAHEDRON, REFINE
% Copyright (C) 1994, Dave Rusin

storeM    = [];
storelen  = [];
increaseM = 0;
while (1)

  % put a single vertex at the top
  phi = [0];
  th  = [0];

  M = round((pi/4)*sqrt(N)) + increaseM;
  for k=1:M
    newphi = (k/M)*pi;
    Q = round(2*M*sin(newphi));
    for j=1:Q
      phi(end+1) = newphi;
      th(end+1)  = (j/Q)*2*pi;
      % in case of even number of contours
      if mod(M,2) && k>(M/2)
        th(end) = th(end) + pi/Q;
      end
    end
  end

  % put a single vertex at the bottom
  phi(end+1) = [pi];
  th(end+1)  = [0];

  % store this vertex packing
  storeM(end+1).th  = th;
  storeM(end  ).phi = phi;
  storelen(end+1) = length(phi);
  if storelen(end)>N
    break;
  else
    increaseM = increaseM+1;
    % fprintf('increasing M by %d\n', increaseM);
  end
end

% take the vertex packing that most closely matches the requirement
[m, i] = min(abs(storelen-N));
th  = storeM(i).th;
phi = storeM(i).phi;

% convert from spherical to cartehsian coordinates
[x, y, z] = sph2cart(th, pi/2-phi, 1);
pos = [x' y' z'];
tri = convhulln(pos);

function [pos1, tri1] = fairsurface(pos, tri, N)

% FAIRSURFACE modify the mesh in order to reduce overlong edges, and
% smooth out "rough" areas. This is a non-shrinking smoothing algorithm.
% The procedure uses an elastic model : At each vertex, the neighbouring
% triangles and vertices connected directly are used. Each edge is
% considered elastic and can be lengthened or shortened, depending
% on their length. Displacement are done in 3D, so that holes and
% bumps are attenuated.
%
% Use as
%   [pos, tri] = fairsurface(pos, tri, N);
% where N is the number of smoothing iterations.
%
% This implements:
%   G.Taubin, A signal processing approach to fair surface design, 1995

% This function corresponds to spm_eeg_inv_ElastM
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging
%                    Christophe Phillips & Jeremie Mattout
% spm_eeg_inv_ElastM.m 1437 2008-04-17 10:34:39Z christophe
%
% $Id$

ts = [];
ts.XYZmm = pos';
ts.tri   = tri';
ts.nr(1) = size(pos,1);
ts.nr(2) = size(tri,1);

% Connection vertex-to-vertex
%--------------------------------------------------------------------------
M_con = sparse([ts.tri(1,:)';ts.tri(1,:)';ts.tri(2,:)';ts.tri(3,:)';ts.tri(2,:)';ts.tri(3,:)'], ...
  [ts.tri(2,:)';ts.tri(3,:)';ts.tri(1,:)';ts.tri(1,:)';ts.tri(3,:)';ts.tri(2,:)'], ...
  ones(ts.nr(2)*6,1),ts.nr(1),ts.nr(1));

kpb   = .1;                       % Cutt-off frequency (default: .1)
lam   = .5; mu = lam/(lam*kpb-1); % Parameters for elasticity. (default: .5)
XYZmm = ts.XYZmm;

% smoothing iterations
%--------------------------------------------------------------------------
for j=1:N

  XYZmm_o = zeros(3,ts.nr(1)) ;
  XYZmm_o2 = zeros(3,ts.nr(1)) ;

  for i=1:ts.nr(1)
    ln = find(M_con(:,i));
    d_i = sqrt(sum((XYZmm(:,ln)-XYZmm(:,i)*ones(1,length(ln))).^2));
    if sum(d_i)==0
      w_i = zeros(size(d_i));
    else
      w_i = d_i/sum(d_i);
    end
    XYZmm_o(:,i) = XYZmm(:,i) + ...
      lam * sum((XYZmm(:,ln)-XYZmm(:,i)*ones(1,length(ln))).*(ones(3,1)*w_i),2);
  end

  for i=1:ts.nr(1)
    ln = find(M_con(:,i));
    d_i = sqrt(sum((XYZmm(:,ln)-XYZmm(:,i)*ones(1,length(ln))).^2));
    if sum(d_i)==0
      w_i = zeros(size(d_i));
    else
      w_i = d_i/sum(d_i);
    end
    XYZmm_o2(:,i) = XYZmm_o(:,i) + ...
      mu * sum((XYZmm_o(:,ln)-XYZmm_o(:,i)*ones(1,length(ln))).*(ones(3,1)*w_i),2);
  end

  XYZmm = XYZmm_o2;

end

% collect output results
%--------------------------------------------------------------------------

pos1 = XYZmm';
tri1 = tri;

if 0
  % this is some test/demo code
  mesh = [];
  [mesh.pos, mesh.tri] = mesh_sphere(162);

  scale = 1+0.3*randn(size(pos,1),1);
  mesh.pos = mesh.pos .* [scale scale scale];

  figure
  ft_plot_mesh(mesh)

  [mesh.pos, mesh.tri] = fairsurface(mesh.pos, mesh.tri, 10);

  figure
  ft_plot_mesh(mesh)
end


function cellInfo = Optical_Flow_Bus(varargin)
% OPTICAL_FLOW_BUS returns a cell array containing bus object information
%
% Optional Input: 'false' will suppress a call to Simulink.Bus.cellToObject
%                 when the MATLAB file is executed.
% The order of bus element attributes is as follows:
%   ElementName, Dimensions, DataType, SampleTime, Complexity, SamplingMode, DimensionsMode, Min, Max, DocUnits, Description

suppressObject = false;
if nargin == 1 && islogical(varargin{1}) && varargin{1} == false
    suppressObject = true;
elseif nargin > 1
    error('Invalid input argument(s) encountered');
end

cellInfo = { ...
  { ...
    'Optical_Flow_Bus', ...
    '', ...
    '', ...
    'Auto', ...
    '-1', {...
{'timestamp', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('ms'), ''}; ...
{'vx', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('m/s'), ''}; ...
{'vy', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('m/s'), ''}; ...
{'quality', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
{'reserved1', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
{'reserved2', 1, 'uint16', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ...
    } ...
  } ...
}';

if ~suppressObject
    % Create bus objects in the MATLAB base workspace
    Simulink.Bus.cellToObject(cellInfo)
end


% [K, dK_logtheta, dK_x2] = gpcov(x1, x2, logtheta)
%
function [K, dK_logtheta, dK_x2] = gpcovDot(logtheta, x1, x2)

warning('Under development, not ready')

% Covariance matrix
K = x1' * x2;

% Gradient for hyperparameters
if nargout >= 2
  dK_logtheta = nan;
end

% Gradients for inputs x2
if nargout >= 3
  if isempty(x2)
    error('Can''t calculate gradient: x2 not given');
  end
  d = rows(x2); % dimensionality of inputs
  m = cols(x1); % number of other inputs
  n = cols(x2); % number of inputs
  dK_x2 = zeros([d,m,n]);
  for j=1:n
    dK_x2(:,:,j) = x1;
  end
end


%% Swan - Wiki
% The student's guide to clean code development
% Task 7: UML of FEM code in Swan repository

% Instructions: run the following code, selecting previously the 'Swan'
% main folder as your current matlab path

file = 'test2d_triangle';
a.fileName = file;
s = FemDataContainer(a);
fem = FEM.create(s);
fem.solve();


%% build_system
% Build a system to solve by MaxwellFDFD.

%%% Syntax
%  [osc, grid3d, s_factor_cell, eps_cell, mu_cell, J_cell] = build_system(ge, OSC, DOM, OBJ, SRC, [progmark])
%  [..., obj_array, src_array, mat_array] = build_system(ge, OSC, DOM, OBJ, SRC, [pragmark])
%  [..., eps_node_array, mu_node_array] = build_system(ge, OSC, DOM, OBJ, SRC, [pragmark])

%%% Description
% |build_system(ge, OSC, DOM, OBJ, SRC, [progmark])| constructs a system from
% given objects and sources.  The constructed system is typically used inside
% <maxwell_run.html maxwell_run>.
%
% |ge| is an instance of |GT| and indicates the grid type of the _E_-field.
% Each following argument, |OSC|, |DOM|, |OBJ|, and |SRC|, represents a
% group of parameters. Each group supports several flexible expressions.
% For more details, see the relevant sections about the input parameter
% groups in <maxwell_run.html |maxwell_run|>.
%
% An additional input parameter |progmark| is an instance of <ProgMark.html
% ProgMark>, which outputs the progress of the system build procedure as the
% standard output.  If it is not given, then it is created internally.
%
% |[osc, grid3d, s_factor_cell, eps_cell, mu_cell, J_cell, M_cell] = build_system(...)|
% returns
%
% * |osc|, an instance of <Oscillation.html Oscillation>
% * |grid3d|, an instance of <Grid3d.html Grid3d>,
% * |s_factor_cell|, a cell array of PML s-factors: |{sx_array, sy_array,
% sz_array}|
% * |eps_cell|, a cell array of electric permittivity evaluated at the E-field
% positions: |{eps_xx_array, eps_yy_array, eps_zz_array}|
% * |mu_cell|,  a cell array of magnetic permeability evaluated at the H-field
% positions: |{mu_xx_array, mu_yy_array, mu_zz_array}|
% * |J_cell|, a cell array of electric current sources: |{Jx_array, Jy_array,
% Jz_array}|
% * |M_cell|, a cell array of electric current sources: |{Mx_array, My_array,
% Mz_array}|
%
% |[..., obj_array, src_array, mat_array] = build_system(...)| returns
% additionally arrays of instances of <EMObject.html |EMObject|>, <Source.html
% |Source|>, and <Material.html |Material|>.  The |EMObject| and |Source|
% elements represent the objects and sources placed in the simulation
% domain, so they can be used to visualize the simulation domain.
%
% |[..., eps_node_array, mu_node_array] = build_system(...)| returns
% additionally arrays of electric permittivity and magnetic permeability
% evaluated at the nodes of the finite-difference grid.

%%% Example
%   gray = [0.5 0.5 0.5];  % [r g b]
%   [osc, grid3d, s_factor_cell, eps_cell, mu_cell, J_cell, M_cell,	...
%       obj_array, src_array, mat_array, eps_node_array, mu_node_array] = build_system(...
%       'OSC', 1e-9, 1550, ...
%       'DOM', {['Palik/SiO2'], 'none'}, [-700, 700; -600, 600; -200, 1700], 20, BC.p, 200, ...
%       'OBJ', ...
%           {['Palik/SiO2'], 'none'}, Box([-50, 50; -50, 50; -200, 1700], [2, 2, 20]), ...  % OBJ1
%           {['CRC/Ag'], gray}, [Box([-700, -25; -25, 25; -200, 1700], 20), Box([25, 700; -25, 25; -200, 1700], 20)], ...  % OBJ2
%       'SRC', PointSrc(Axis.x, [0, 0, 200]) ...
%       );

function [osc, grid3d, s_factor_cell, eps_cell, mu_cell, J_cell, M_cell, ...
	obj_array, src_array, mat_array, eps_node, mu_node, isiso] = build_system(varargin)

	iarg = nargin; arg = varargin{iarg};
	if istypesizeof(arg, 'ProgMark')
		pm = arg;
		narglim = nargin - 1;
	else
	 	pm = ProgMark();
		varargin = [varargin, {pm}];
		narglim = nargin;
	end

	iarg = 1; arg = varargin{iarg};
	chkarg(istypesizeof(arg, 'GT'), 'argument #%d should be "ge" (GT).', iarg);
	ge = arg;

	iarg = iarg + 1; arg = varargin{iarg};
	chkarg(istypesizeof(arg, 'PML'), 'argument #%d should be "pml" (PML).', iarg);
	pml = arg;

	function material = create_material(varargin)
		narg = nargin;
		if istypesizeof(varargin{end}, 'logical')
			narg = narg - 1;
		end

		chkarg(narg >= 2, '# of arguments should be at least 2.')
		matname = varargin{1};
		if isempty(strfind(matname, '/'))  % data table is not specified
			material = Material(varargin{:});
		else  % data table is specified
			material = Material.fromtable(osc, varargin{:});
		end
	end

	osc = Oscillation.empty();
	obj_dom = EMObject.empty();
	shape_array = Shape.empty();
	sshape_array = Shape.empty();
	mat_array = Material.empty();
	obj_array = EMObject.empty();
	sobj_array = EMObject.empty();
	srcj_array = [];
	srcm_array = [];
	isepsgiven = false;
	isTFSF = false;
	while iarg < narglim
		iarg = iarg + 1; arg = varargin{iarg};

		if ischar(arg) && strcmpi(arg,'OSC')
			% Set up OSC.
			iarg = iarg + 1; arg = varargin{iarg};
			chkarg((istypesizeof(arg, 'real') && arg > 0) || istypesizeof(arg, 'Oscillation'), ...
				'"argument #%d should be either "L0" (positive) or "osc" (instance of Oscillation).', iarg);
			if istypesizeof(arg, 'real')
				L0 = arg;
				iarg = iarg + 1; wvlen = varargin{iarg};
				chkarg(istypesizeof(wvlen, 'complex'), 'argument #%d should be "wvlen" (complex).', iarg);
				unit = PhysUnit(L0);
				osc = Oscillation(wvlen, unit);
			else  % arg is instance of Oscillation
				osc = arg;
			end
		elseif ischar(arg) && strcmpi(arg,'DOM')
			% Set up DOM.
			iarg = iarg + 1; arg = varargin{iarg};
			chkarg(iscell(arg) || istypesizeof(arg, 'Material') || istypesizeof(arg, 'EMObject'), ...
				'argument #%d should be cell, instance of Material, or instance of EMObject.', iarg);
			if istypesizeof(arg, 'EMObject')
				obj_dom = arg;
				domain = obj_dom.shape;
				chkarg(istypesizeof(domain, 'Domain'), 'argument #%d should be instance of EMObject with Domain as its shape.', iarg);
			else
				if iscell(arg)
					mat_dom = create_material(arg{:});
				else
					assert(istypesizeof(arg, 'Material'));
					mat_dom = arg;
				end

				iarg = iarg + 1; arg = varargin{iarg};
				chkarg(istypesizeof(arg, 'real', [Axis.count, Sign.count]) || istypesizeof(arg, 'Domain'), ...
					'argument #%d should be either "box_dom" ([xmin xmax; ymin ymax; zmin zmax]) or "domain" (instance of Domain).', iarg);
				if istypesizeof(arg, 'real', [Axis.count, Sign.count])
					box_domain = arg;
					iarg = iarg + 1; dl_domain = varargin{iarg};
					chkarg(istypeof(dl_domain, 'real') && isexpandable2row(dl_domain, Axis.count), ...
						'"argument #%d should be dl_domain (positive number or length-%d row vector of positive numbers).', iarg, Axis.count);
					domain = Domain(box_domain, expand2row(dl_domain, Axis.count));
				else  % arg is instance of Domain
					domain = arg;
				end
				obj_dom = EMObject(domain, mat_dom);
			end
			mat_array = [mat_array(1:end), obj_dom.material];

			% Set up boundary conditions and PML thicknesses.
			iarg = iarg + 1; bc = varargin{iarg};
			chkarg(istypeof(bc, 'BC') && isexpandable2mat(bc, Axis.count, Sign.count), ...
				'argument #%d should be "bc" (scalar, length-%d row vector, or %d-by-%d matrix with BC as elements).', iarg, Axis.count, Axis.count, Sign.count);
			iarg = iarg + 1; Lpml = varargin{iarg};
			chkarg(istypeof(Lpml, 'real') && isexpandable2mat(Lpml, Axis.count, Sign.count) && all(all(Lpml>=0)), ...
				'argument #%d should be "Lpml" (scalar, length-%d row vector, or %d-by-%d matrix with nonnegative numbers as elements).', iarg, Axis.count, Axis.count, Sign.count);

			% Set up the degree of the polynomial grading of the PML scale
			% factors.
			iarg = iarg + 1; arg = varargin{iarg};
			deg_pml = 4;  % polynomial degree
			if istypeof(arg, 'real')
				deg_pml = arg;
			else
				iarg = iarg - 1; % because deg_pml is optional argument
			end

			% Set up the target reflection coefficient of the PML.
			iarg = iarg + 1; arg = varargin{iarg};
			R_pml = exp(-16);  % target reflection coefficient
			if istypeof(arg, 'real')
				R_pml = arg;
			else
				iarg = iarg - 1; % because R_pml is optional argument
			end

			% Set up a flag to generate a grid dynamically.
			iarg = iarg + 1; arg = varargin{iarg};
			withuniformgrid = false;  % generate a grid dynamically by default.
			if istypesizeof(arg, 'logical')
				withuniformgrid = arg;
			else
				iarg = iarg - 1; % because withuniformgrid is optional argument
			end
		elseif ischar(arg) && (strcmpi(arg,'OBJ') || strcmpi(arg,'SOBJ'))
			% Set up OBJ.
			is_scatterer = strcmpi(arg,'SOBJ');
			iarg = iarg + 1; arg = varargin{iarg};
			if istypesizeof(arg, 'complex', [0 0 0])  % 3D complex array with arbitrary size
				isepsgiven = true;
				eps_node_cell = {arg, arg, arg};
				mu_node_temp = ones(size(arg));
				mu_node_cell = {mu_node_temp, mu_node_temp, mu_node_temp};
			else
				% Set up objects.
				obj_array_temp = EMObject.empty();
				shape_array_temp = Shape.empty();
				while iscell(arg) || istypesizeof(arg, 'Material') || istypesizeof(arg, 'EMObject', [1 0])
					if istypesizeof(arg, 'EMObject', [1 0])
						objs = arg;
						obj_array_temp = [obj_array_temp(1:end), objs];
						for obj = objs
							shape_array_temp = [shape_array_temp(1:end), obj.shape];
							mat_array = [mat_array(1:end), obj.material];
						end
						iarg = iarg + 1; arg = varargin{iarg};
					else
						if iscell(arg)
							mat = create_material(arg{:});
						else
							assert(istypesizeof(arg, 'Material'));
							mat = arg;
						end
						mat_array = [mat_array(1:end), mat];

						iarg = iarg + 1; arg = varargin{iarg};
						while istypesizeof(arg, 'Shape', [1 0])
							shapes = arg;
							shape_array_temp = [shape_array_temp(1:end), shapes];
							objs = EMObject(shapes, mat);
							obj_array_temp = [obj_array_temp(1:end), objs];
							iarg = iarg + 1; arg = varargin{iarg};
						end
					end
				end
				iarg = iarg - 1;

				if is_scatterer
					sshape_array = [sshape_array(1:end), shape_array_temp];
					sobj_array = [sobj_array(1:end), obj_array_temp];
				else
					shape_array = [shape_array(1:end), shape_array_temp];
					obj_array = [obj_array(1:end), obj_array_temp];
				end
			end
		elseif ischar(arg) && strcmpi(arg,'SRCJ')
			% Set up sources.
			iarg = iarg + 1; arg = varargin{iarg};
			if ~istypesizeof(arg, 'Source', [1 0])
				warning('Maxwell:buildSys', 'no source is given.');
			end

			while istypesizeof(arg, 'Source', [1 0])
				if istypesizeof(arg, 'TFSFPlaneSrc')
					isTFSF = true;
				end
				srcj_array_curr = arg;
				for src = srcj_array_curr
					src.set_gridtype(ge);
				end
				srcj_array = [srcj_array(1:end), srcj_array_curr];
				iarg = iarg + 1; arg = varargin{iarg};
			end
			iarg = iarg - 1;
		elseif ischar(arg) && strcmpi(arg,'SRCM')
			% Set up sources.
			iarg = iarg + 1; arg = varargin{iarg};
			if ~istypesizeof(arg, 'Source', [1 0])
				warning('Maxwell:buildSys', 'no source is given.');
			end

			while istypesizeof(arg, 'Source', [1 0])
				if istypesizeof(arg, 'TFSFPlaneSrc')
					isTFSF = true;
				end
				srcm_array_curr = arg;
				for src = srcm_array_curr
					src.set_gridtype(alter(ge));
				end
				srcm_array = [srcm_array(1:end), srcm_array_curr];
				iarg = iarg + 1; arg = varargin{iarg};
			end
			iarg = iarg - 1;
		elseif iarg == narglim
			chkarg(false, ['some arguments are not used.\n', ...
				'Suggestion: check if each parameter group is specified with beginning specifier.']);
		end
	end
	chkarg(~isempty(osc), 'OSC parameter groups should be set.');
	chkarg(~isempty(obj_dom), 'DOM parameter groups should be set.');
	obj_array = [obj_dom, obj_array];
	src_array = [srcj_array, srcm_array];
	if isTFSF && isempty(sobj_array)
		warning('Maxwell:objAssign', 'TF/SF source is used, but scatteres are not defined in SOBJ group.');
	end

	chkarg(iarg <= narglim, 'more arguments than expected.');
	pm.mark('initial setup');

	fprintf('\tLength Unit: %s m\n', osc.unit.value(PhysQ.L));
	fprintf('\twvlen = %s, freq = %s eV\n', num2str(osc.in_L0()), num2str(osc.in_eV()));

	mat_array = unique(mat_array);
	isiso = true;
	fprintf('materials used:\n');
	for mat = mat_array
		epstext = mat.eps;
		if length(unique(epstext)) == 1
			epstext = epstext(Axis.x);
		end

		mutext = mat.mu;
		if length(unique(mutext)) == 1
			mutext = mutext(Axis.x);
		end

		fprintf('\t%s: eps = %s, mu = %s\n', mat.name, mat2str(epstext), mat2str(mutext));

		isiso = isiso && mat.isiso;
	end

	% Generate a grid.
	[lprim, Npml] = generate_lprim3d(domain, Lpml, [shape_array, sshape_array], src_array, withuniformgrid);
	grid3d = Grid3d(osc.unit, lprim, Npml, bc);
	if withuniformgrid
		pm.mark('uniform grid generation');
	else
		pm.mark('nonuniform grid generation');
	end
	fprintf('\t[Nx Ny Nz] = %s\n', mat2str(grid3d.N));

	% Generate a warning when a seemingly 2D simulation is defined on a 3D grid.
	[like2d, normal_axis] = is2dlike(grid3d.N);
	if like2d && grid3d.N(normal_axis) >= 2  % possible user mistake
		warning(['If this a 2D structure, N%s should be 1; ', ...
			'check d%s''s of objects and locations of sources'], char(normal_axis), char(normal_axis));
	end

	% Construct material parameters.
	if ~isepsgiven
		[eps_node_cell, mu_node_cell] = assign_material_node(grid3d, obj_array);  % Nx x Ny x Nz
	end
	eps_cell = mean_material_node(grid3d, ge, eps_node_cell);
	mu_cell = mean_material_node(grid3d, alter(ge), mu_node_cell);

	% Construct PML s-factors.
	s_factor_cell = generate_s_factor(osc.in_omega0(), grid3d, deg_pml, R_pml);
	pm.mark('eps and mu assignment');

	if ~isTFSF
		% Solve for modes.
		for src = src_array
			if istypesizeof(src, 'ModalSrc')
				modalsrc = src;
				if ~modalsrc.ispreped
					prep_modalsrc(ge, pml, osc, grid3d, eps_cell, mu_cell, s_factor_cell, modalsrc);
				end

				neff = modalsrc.neff;
				beta = 2*pi*neff / osc.in_L0();
				pm.mark('mode calculation');
				fprintf('\tbeta = %s, n_eff = %s\n', num2str(beta), num2str(neff));
			end
		end
	else  % isTFSF == true
		% Set up J for TF/SF.
		for src = src_array
			if istypesizeof(src, 'TFSFPlaneSrc')
				tfsfsrc = src;
				cb_center = num2cell(tfsfsrc.shape.cb_center);
				for bgobj = fliplr(obj_array)
					if bgobj.shape.contains(cb_center{:})
						break;  % assume that last object containing TF box center fills TF box
					end
				end
				tfsfsrc.set_bg_material(bgobj.material);
				F0 = tfsfsrc.create_incidentF(osc, grid3d);
				JM = cell(1, Axis.count);
				for w = Axis.elems
					JM{w} = zeros(grid3d.N);
				end

				if tfsfsrc.gt == ge
					eqtype_tfsf = EquationType(FT.e, ge);  % for SRCJ, create E-field eq
				else
					eqtype_tfsf = EquationType(FT.h, ge);  % for SRCM, create H-field eq
				end
% 				A = create_eq(eqtype_tfsf, pml, osc.in_omega0(), eps_cell, mu_cell, s_factor_cell, JM, JM, grid3d);
				eq = MatrixEquation(eqtype_tfsf, pml, osc.in_omega0(), eps_cell, mu_cell, s_factor_cell, JM, JM, grid3d);
				Op = eq.matrixfree_op();

				x0 = [F0{Axis.x}(:); F0{Axis.y}(:); F0{Axis.z}(:)];
				r = reordering_indices(Axis.count, grid3d.N);
				x0 = x0(r);

% 				JM = (A*x0) ./ (-1i*osc.in_omega0());
				JM = Op(x0, 'notransp') ./ (-1i*osc.in_omega0());
				JM = reshape(JM, [Axis.count grid3d.N]);
				JM = permute(JM, [Axis.elems+1, 1]);
				JM = {JM(:,:,:,Axis.x), JM(:,:,:,Axis.y), JM(:,:,:,Axis.z)};

				tfsfsrc.setJM(JM, grid3d);
			end
		end

		% Add sobj_array to the already-generated eps and mu.
		[eps_node_cell, mu_node_cell] = assign_material_node(grid3d, sobj_array, eps_node_cell, mu_node_cell);  % Nx x Ny x Nz
		eps_cell = mean_material_node(grid3d, ge, eps_node_cell);  % Nx x Ny x Nz
		mu_cell = mean_material_node(grid3d, alter(ge), mu_node_cell);  % Nx x Ny x Nz

		pm.mark('TF/SF source assignment');
	end
	obj_array = [obj_array, sobj_array];

	eps_node = cell(1, Axis.count);
	mu_node = cell(1, Axis.count);
	for w = Axis.elems
		eps_node_cell{w} = expand_node_array(grid3d, eps_node_cell{w});  % (Nx+2) x (Ny+2) x (Nz+2)
		mu_node_cell{w} = expand_node_array(grid3d, mu_node_cell{w});  % (Nx+2) x (Ny+2) x (Nz+2)

		eps_node{w} = Scalar3d(eps_node_cell{w}, grid3d, [GT.dual GT.dual GT.dual], osc, PhysQ.eps, '\epsilon');
		mu_node{w} = Scalar3d(mu_node_cell{w}, grid3d, [GT.dual GT.dual GT.dual], osc, PhysQ.mu, '\mu');
	end

	% Construct sources.
	J_cell = assign_source(grid3d, srcj_array);
	M_cell = assign_source(grid3d, srcm_array);
	pm.mark('J assignment');
end


function v3d_write_volume(I,fname,scales)
% Function for writing V3D volume files version R6.1
%
% v3d_write_volume(volume, filename, voxelsize in mm)
%
% examples:
% I=uint16(rand(64,64,64)*65536);
%
% 1: v3d_write_volume(I);
% 2: v3d_write_volume(I,'random.v3d',[2 2 2];

% Filename
    if((exist('fname','var')==0)),
        [filename, pathname] = uiputfile('*.v3d', 'Write v3d-file');
        fname = [pathname filename];
    end

% Sizes
    sizes=size(I);
% Scales
    if(exist('scales','var')==0), scales=ones(1,3); end;
% Offset
    offset=204; % Header size R6.1
% Filesize
    fsize=numel(I)*2+offset;
% Format
    fform='3D-RA';
% Version
    vers='6.1';
% Voxelbits
    bits=16;
% par1;
    par1 = 1072693248; % The meaning of this number is unknown

disp(['filename : ' num2str(fname)]);
disp(['format : ' fform]);
disp(['version : ' vers]);
disp(['filesize : ' num2str(fsize)]);
fprintf('sizes : %i, %i, %i\n',sizes);
fprintf('scales : %2.6f, %2.6f, %2.6f\n',scales);
disp(['voxelbits : ' num2str(bits)]);
disp(['offset : ' num2str(offset)]);
disp(['par1 : ' num2str(par1)]);
fprintf('\n');

fout=fopen(fname,'wb');
fwrite(fout,[fform ' R' vers],'char');
fwrite(fout,zeros(1,30),'uint8'); %seek
fwrite(fout,sizes,'int');
fwrite(fout,scales,'double');

fwrite(fout,zeros(1,28),'uint8'); %seek
fwrite(fout,par1,'int');
fwrite(fout,zeros(1,28),'uint8'); %seek
fwrite(fout,par1,'int');
fwrite(fout,zeros(1,28),'uint8'); %seek
fwrite(fout,par1,'int');

fwrite(fout,bits,'int'); % Seems the new (R6.1) number of bits position

% Meaning of this part of the header is unknown
fwrite(fout,140,'int');
fwrite(fout,1,'int');
fwrite(fout,1075789855,'int');
fwrite(fout,1068449823,'int');
fwrite(fout,0,'int');
fwrite(fout,0,'uint8');
fwrite(fout,64,'uint8');
fwrite(fout,143,'uint8');
fwrite(fout,192,'uint8');
fwrite(fout,1,'int');

% Write uint16 volume
fwrite(fout,uint16(I),'uint16');

fclose('all');


function [dat] = read_nex_data(filename, hdr, begsample, endsample, chanindx)

% READ_NEX_DATA for Plexon *.nex file
%
% Use as
%   [dat] = read_nex_data(filename, hdr, begsample, endsample, chanindx)
%
% See also READ_NEX_HEADER, READ_NEX_EVENT

% Copyright (C) 2007, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

try,
  % work with the original header, not the FieldTrip one
  hdr = hdr.orig;
catch
  % assume that we got the original header
end

numsmp = cell2mat({hdr.varheader.numsmp});
adindx = find(cell2mat({hdr.varheader.typ})==5);
smpfrq = hdr.varheader(adindx(1)).wfrequency;
sgn    = chanindx;
nsmp   = (endsample-begsample+1);
dat    = zeros(length(sgn), nsmp);

fid = fopen_or_error(filename, 'r', 'ieee-le');
for sgnlop=1:length(sgn)

  if hdr.varheader(sgn(sgnlop)).typ == 0
    % read a spike channel
    status = fseek(fid,hdr.varheader(sgn(sgnlop)).offset,'bof');

    % read the sample indices at which spikes occurred
    tim = fread(fid,hdr.varheader(sgn(sgnlop)).cnt,'int32');
    % downsample from 40kHz to the A/D sampling frequency
    tim = (tim ./ hdr.filheader.frequency) * smpfrq + 1;  % add one sample, since ts=0 corresponds to sample=1
    tim = round(tim);   % needed because of the edges in histc
    % select only samples between the desired begin and end
    % tim = tim(find(tim>=begsample & tim<=endsample));
    % convert sample indices into a continuous signal
    if ~isempty(tim)
      dum = histc(tim-begsample, 0:(nsmp-1), 1);
      dat(sgnlop,:) = dum(:)';
    end

  elseif hdr.varheader(sgn(sgnlop)).typ == 5
    % read an A/D channel
    status = fseek(fid,hdr.varheader(sgn(sgnlop)).offset,'bof');

    % this just reads the times of LFP starts
    tim = fread(fid,hdr.varheader(sgn(sgnlop)).cnt,'int32');
    % this just reads the indices of LFP starts
    ind = fread(fid,hdr.varheader(sgn(sgnlop)).cnt,'int32');
    if length(ind)>1
      ft_error('multiple A/D segments are not supported');
    end

    % convert from timestamps to samples, expressed in the sampling frequency of the AD channels
    tim = (tim ./ hdr.filheader.frequency) * smpfrq;
    tim = round(tim);

    ch_begsample = begsample - tim;
    ch_endsample = endsample - tim;

    if (ch_begsample<1)
      ft_error(sprintf('cannot read before the begin of the recorded data (channel %d)', sgn(sgnlop)));
    elseif (ch_endsample>hdr.varheader(sgn(sgnlop)).numsmp)
      ft_error(sprintf('cannot read beyond the end of the recorded data (channel %d)', sgn(sgnlop)));
    end

    % seek to the beginning of the interesting data, correct for the A/D card initialisation delay
    fseek(fid,(ch_begsample-1)*2, 'cof');
    % read the actual data for the whole channel
    dum = fread(fid,nsmp,'int16');
    % convert to mV
    dat(sgnlop,:) = dum(:)' * hdr.varheader(sgn(sgnlop)).adtomv;

  else
    % ft_warning('unsupported data format for channel %s', hdr.label{sgn(sgnlop)});
  end

end
status = fclose(fid);


function features = LoadSpikeFeatures(filename,rate)

%LoadSpikeFeatures - Load spike features from file.
%
%  USAGE
%
%    features = LoadSpikeFeatures(filename,rate)
%
%    filename            spike file name (either .clu, .res or .fet)
%    rate                sampling rate
%
%  OUTPUT
%
%    The output is a list of (timestamp,group,cluster,features...) t-uples.
%
%  SEE
%
%    See also GetSpikeFeatures.

% Copyright (C) 2004-2011 by Michaël Zugaro
%
% 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.

[path,basename,extension] = fileparts(filename);
if isempty(path), path = '.'; end

electrodeGroup = str2num(extension(2:end));
[unused,basename,unused] = fileparts(basename);

% Load .clu file
filename = [path '/' basename '.clu.' int2str(electrodeGroup)];
if ~exist(filename),
	error(['File ''' filename ''' not found.']);
end
clu = load(filename);
clu = clu(2:end);

% Load .fet file
filename = [path '/' basename '.fet.' int2str(electrodeGroup)];
if ~exist(filename),
	error(['File ''' filename ''' not found.']);
end
file = fopen(filename,'r');
if file == -1,
	error(['Cannot open file ''' filename '''.']);
end
nFeatures = fscanf(file,'%d',1);
fet = fscanf(file,'%f',[nFeatures,inf])';
fclose(file);

features = [fet(:,end)/rate electrodeGroup*ones(size(clu)) clu fet(:,1:end-1)];


function optsetup = spm_cfg_eeg_inv_optimize
%function optsetup = spm_cfg_eeg_inv_optimize
% configuration file to set up optimization routines for M/EEG source
% inversion
%_______________________________________________________________________
% Copyright (C) 2010 Wellcome Trust Centre for Neuroimaging

% Gareth Barnes
% $Id: spm_cfg_eeg_inv_optimize.m 6499 2015-07-16 13:37:41Z gareth $

D = cfg_files;
D.tag = 'D';
D.name = 'M/EEG datasets';
D.filter = 'mat';
D.num = [1 Inf];
D.help = {'Select the M/EEG mat files.'};

val = cfg_entry;
val.tag = 'val';
val.name = 'Inversion index';
val.strtype = 'n';
val.help = {'Index of the cell in D.inv where the forward model can be found and the results will be stored.'};
val.val = {1};

priorname = cfg_entry;
priorname.tag = 'priorname';
priorname.name = 'Prefix for priors';
priorname.strtype = 's';
priorname.num = [1 Inf];
priorname.val = {'priorset1'};
priorname.help = {'Prefix for prior directory'};

REMLopt = cfg_entry;
REMLopt.tag = 'REMLopt';
REMLopt.name = 'REML parameters';
REMLopt.strtype = 'r';
REMLopt.num = [1 2];
REMLopt.val = {[-4,16]};
REMLopt.help = {'Select REML parameters'};

ARDopt = cfg_entry;
ARDopt.tag = 'ARDopt';
ARDopt.name = 'Threshold for ARD hyperparameter';
ARDopt.strtype = 'r';
ARDopt.num = [1 1];
ARDopt.val = {[128]};
ARDopt.help = {'ARD threshold for pruning hyperparameters (relative to max)'};

GSopt = cfg_entry;
GSopt.tag = 'GSopt';
GSopt.name = 'Set number of greed search iterations';
GSopt.strtype = 'i';
GSopt.num = [1 1];
GSopt.val = {[16]};
GSopt.help = {'Number of greedy search iterations'};

opttype = cfg_repeat;
opttype.tag = 'opttype';
opttype.name = 'Optimize';
opttype.help = {'Specify the optimization scheme'};
opttype.num  = [1 Inf];
opttype.values  = {REMLopt,ARDopt,GSopt};
opttype.val  = {REMLopt};

optsetup = cfg_exbranch;
optsetup.tag = 'optsetup';
optsetup.name = 'Inversion optimization';
optsetup.val = {D,val,priorname,opttype};
optsetup.help = {'Set optimization scheme for source reconstruction'};
optsetup.prog = @opt_priors;
optsetup.vout = @vout_opt_priors;

function  out = opt_priors(job)

D = spm_eeg_load(job.D{1});

%% get data specific terms sorted out
inverse=D.inv{job.val}.inverse;
AY=inverse.AY;

mesh=D.inv{job.val}.mesh.tess_mni;

[a1,b1,c1]=fileparts(D.fname);
priordir=[D.path filesep job.priorname '_' b1];

[priorfiles] = spm_select('FPListRec',priordir,'.*\.mat$');
%[postfiles] = spm_select('FPListRec',priordir,'^post.*\.mat$');

% USEPOST=1;
% if ~isempty(postfiles) && USEPOST,
%     fprintf('Using only posterior\n');
%     priorfiles=postfiles;
% end;
Nfiles=size(priorfiles,1);
fprintf('Found %d prior files\n',Nfiles);
if Nfiles==0,
    error('No prior file found in directory: %s', priordir);
end;

%% add in functional (from other modalities or experiment) hypotheses

Qe0=0;%% bounding ratio of noise to signal power
disp('NB NO min sensor noise level');  %% NO MIN SENSOR NOISE LEV

Qp_best=[];
Qe_best=[];
Fmax=-Inf;
F_aug=-Inf;
maxpriors=512;

for j=1:Nfiles, %% move through prior files
    %%% LOAD IN PRIOR FILE

    load(deblank(priorfiles(j,:)),'Qp','Qe','UL','F');
    fprintf('Optimizing priorfile %d of %d \n',j,Nfiles);
    %% ALSO CONSIDER AUGMENTED VERSION OF PRIORS IN FILE WITH BEST SO FAR
    Qp_aug=Qp; %% running with augmented Qp
    for k=1:length(Qp_best), %
        Qp_aug{length(Qp)+k}=Qp_best{k}; % augment with posterior from best so far
    end;
    Qe_aug=Qe; %% NB NOT AUGMENTING YET

    if length(Qp_aug)>maxpriors,
        Qp_aug=Qp_aug{1:maxpriors};
        warning('Limiting priors');
    end;
    %%%%%%%%%%%% NOW OPTIMIZE ORIGINAL AND AUGMENTED INDEPENDENTLY

    for k=1:length(job.opttype), %% move through optimization schemes
        %%% TERMS WHICH EVOLVE ARE Qe and Qp (M, Cq, Cp follow)
        priorcount(k)=length(Qp);

        [F,M,Cq,Cp,Qe,Qp] = spm_eeg_invert_EBoptimise(AY,UL,job.opttype(k),Qp,Qe,Qe0);

        if j>1, %% nothing to augment on 1st iteration

            [F_aug,M,Cq,Cp,Qe_aug,Qp_aug] = spm_eeg_invert_EBoptimise(AY,UL,job.opttype(k),Qp_aug,Qe_aug,Qe0);
        end;
    end;

    if F_aug>F,
        fprintf('Taking augmented set forward\n');
        Qp=Qp_aug;
        Qe=Qe_aug;
        F=F_aug;
    end;

    if F>Fmax, %% keep a record of these if they are best
        Qp_best=Qp;
        Qe_best=Qe;
        Fmax=F;
    end;

    Qp=Qp_best;
    Qe=Qe_best;
    save(deblank(priorfiles(j,:)),'Qp','Qe','UL','F');
    allF(j)=F;
end; % for j

%% Now get M, Cp, Cq etc based on Qp and Qe

[LCpL,Q,sumLCpL,QE,Cy,M,Cp,Cq,Lq]=spm_eeg_assemble_priors(UL,Qp,{Qe});

inverse.F=Fmax;
inverse.M=M;
inverse.qC=Cq;
inverse.Cp=Cp; %% posterior source level
inverse.Qe=Qe; %% posterior sensor level
inverse.Qp=Qp;
inverse.Is=1:size(Cp,1); % %% temporary fix

%----------------------------------------------------------------------
% evaluate conditional expectation (of the sum over trials)
%----------------------------------------------------------------------
SSR   = 0;
SST   = 0;
J     = {};

UY=inverse.UY;

sourcevar=zeros(1,size(inverse.M,1));
for j = 1:numel(UY),

    % trial-type specific source reconstruction
    %------------------------------------------------------------------
    J{j} = inverse.M*UY{j};
    Jtime=J{j}*inverse.T'; %% J is Nvert* Nsamples
    sourcevar=sourcevar+var(Jtime'); %% ./inverse.qC';
    % sum of squares
    %------------------------------------------------------------------
    SSR  = SSR + sum(var((UY{j} - UL*J{j}))); %% changed variance calculation
    SST  = SST + sum(var( UY{j}));

end

inverse.J=J;

% accuracy; signal to noise (over sources)
%======================================================================
inverse.R2   = 100*(SST - SSR)/SST;
fprintf('Percent variance explained %.2f\n',full(inverse.R2));
D.inv{job.val}.inverse=inverse;

spm_eeg_invert_display(D);

rmind=find(allF<max(allF)-3);
fprintf('Removing %d poorest prior files\n',length(rmind));
for j=1:length(rmind),
    fprintf('Deleting %s\n',priorfiles(rmind(j),:));
    delete(deblank(priorfiles(rmind(j),:)));
end;

idnum=round(spm_data_id(sourcevar)*1000); %% get unique id for file
postfilename=[priordir filesep sprintf('post%d.mat',idnum)];
postgiftiname=[priordir filesep sprintf('post%d.gii',idnum)];
fprintf('Making posterior %s based on diagonal\n',postfilename);
Qp=[];
Qp{1}=sparse(diag(sourcevar));
Mmni=[];
Mmni.faces=D.inv{job.val}.mesh.tess_mni.face;
Mmni.vertices=D.inv{job.val}.mesh.tess_mni.vert;
Mmni.cdata=sourcevar';
Mmni=gifti(Mmni);

save(deblank(postfilename),'Qp','Qe','UL','Fmax','postgiftiname','-v7.3');
save(Mmni,postgiftiname);

figure;
spm_mip(sourcevar,mesh.vert',6);
title('Posterior');
colorbar;

D.save;
out.postname=postfilename;
out.postgiftiname=postgiftiname;
out.D = job.D;

function dep = vout_opt_priors(job)
% Output is always in field "D", no matter how job is structured
dep = cfg_dep;
dep.sname = 'M/EEG dataset(s) after imaging source reconstruction';
% reference field "D" from output
dep.src_output = substruct('.','D');
% this can be entered into any evaluated input
dep.tgt_spec   = cfg_findspec({{'filter','mat'}});


function out = spm_run_bms_map (job)
% Run Bayesian Model Selection Maps
% SPM job execution function
% takes a harvested job data structure and calls SPM functions to perform
% Bayesian Inference for Model Selection of Log. Evidence Maps
% Input:
% job    - harvested job data structure (see matlabbatch help)
% Output:
% out    - computation results, usually a struct variable.
%
%
% Bayesian Inference on Model Space:
%
% The Random-effects 'RFX' method is described in Stephan et al. [1]
% 'Bayesian Model Selection for Group Studies'.
% Output files (for each model):
%       BMS.mat
%       Exceedance Probability Maps (*epm.<ext>),
%       Posterior Probability Maps (*ppm.<ext>),
%       Dirichlet Paramters (alpha) Maps (*alpha.<ext>).
%
% The Fixed-effects 'FFX' method adds together the log-evidences over
% subjects/sessions for each group, then compares the group log-ev's.
% This is also known as the Group Bayes Factor (GBF) approach [2].
% Output files (for each model):
%       BMS.mat
%       Posterior Probability Maps (*ppm.<ext>).
%
% BMS contains:
%     BMS.fname
%     BMS.map.ffx(rfx).data
%     BMS.map.ffx(rfx).ppm
%     BMS.map.ffx(rfx).xppm     - only for RFX (this is the expected posterior
%                                 probability map ie. posterior mean)
%     BMS.map.ffx(rfx).epm      - only for RFX (optional) - this is the
%                                 exceedance probability map
%     BMS.map.ffx(rfx).alpha    - only for RFX
%
% [1] Rosa et al., 2009, Bayesian Model Selection Maps for Group Studies,
% NeuroImage.
% [2] Stephan et al., 2009, Bayesian Model Selection for Group Studies,
% NeuroImage.
% [3] Penny et al., 2004, Comparing Dynamic Causal Models, NeuroImage.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Maria Joao Rosa
% $Id: spm_run_bms_map.m 5740 2013-11-13 12:00:04Z guillaume $

% Input
% -------------------------------------------------------------------------
direct  = job.dir{1};
fname   = fullfile(direct,'BMS.mat'); % Output filename (with full path)
mask    = length(job.mask{1});        % Mask image
if mask
   mask_image = spm_vol(job.mask);    % Mask image Vol
end
nsamps    = str2num(job.nsamp);       % Number of samples (nmodels > 3)
do_maps   = job.out_file;
do_ecp    = do_maps > 0;              % Compute Exceedance Probability
do_alpha  = do_maps > 1;              % Compute Alpha Parameters

% Nb. of subjects and models
% -------------------------------------------------------------------------
nsubjs    = size(job.sess_map,2);
nmodels   = size(job.sess_map{1}(1).mod_map,1);
nsess     = size(job.sess_map{1},2);
nnames    = size(job.mod_name,2);
names     = job.mod_name;

% method
% -------------------------------------------------------------------------
if strcmp(job.method_maps,'FFX');
    method = 'FFX';
else
    method = 'RFX';
end

% Name models
% -------------------------------------------------------------------------
if nnames < nmodels
    for nn=1:nmodels-nnames
        names = [names, sprintf('m%d',nn)];
    end
end

if size(unique(names(1:nmodels)),2) < nmodels,
    id = 'Indentical names for different models!';  % Same name!
    error(id);
end

if nsubjs == 1
   method = 'FFX';                              % If only 1 subject do FFX
end

% Sort out log-evidence images dimensions
% -------------------------------------------------------------------------
Vol_models(1,1) = spm_vol(job.sess_map{1}(1).mod_map(1));

first_vol       = Vol_models(1,1);
M               = first_vol{1}.mat;
DIM             = first_vol{1}.dim(1:3)';

xdim            = DIM(1);
ydim            = DIM(2);
zdim            = DIM(3);
[xords,yords]   = ndgrid(1:xdim,1:ydim);
xords           = xords(:)';
yords           = yords(:)';
I               = 1:xdim*ydim;
zords_init      = ones(1,xdim*ydim);

% Setup images
% -------------------------------------------------------------------------
switch method

    case 'FFX',   % Fixed Effects

        % Check if BMS.mat exists
        if exist(fullfile(job.dir{1},'BMS.mat'),'file')
           load(fname);
           if  isfield(BMS,'map') && isfield(BMS.map,'ffx')
               str = { 'Warning: existing BMS.mat file has been over-written!'};
               msgbox(str)
           end
        end

        % Save BMS data
        out.files{1} = fname;

        % Create PPM image files for each model
        model_ppm(1:nmodels) = struct(...
        'fname',    '',...
        'dim',      DIM',...
        'dt',       [spm_type('float32') spm_platform('bigend')],...
        'mat',      M,...
        'pinfo',    [1 0 0]',...
        'n', [1 1], ...
        'descrip',  '');

        % Load Vols for all subjects/models
        for i = 1:nmodels
            model_ppm(i).fname   = fullfile(direct,[sprintf('%s_model_ppm',names{i}) spm_file_ext]);
            model_ppm(i).descrip = sprintf('PPM: %s model',names{i});
            BMS.map.ffx.ppm{i}   = model_ppm(i).fname;

            for s = 1:nsubjs,
                for se = 1:nsess,
                    nsessi      = size(job.sess_map{s},2);
                    nmodelsi    = size(job.sess_map{s}(se).mod_map,1);
                    if (nsess == nsessi && nmodels == nmodelsi)
                        Vol_models(s,i,se) = spm_vol(job.sess_map{s}(se).mod_map(i));
                        tmp = Vol_models(s,i,se);
                    else
                        msgbox('The number of sessions/models should be the same for all subjects!')
                        return
                    end
                    % Stop if log-ev images have different dimensions
                    if tmp{1}.dim(1)~=xdim || tmp{1}.dim(2)~=ydim || tmp{1}.dim(3)~=zdim
                       error('Log-evidence images must have the same dimensions!')
                    end
                end
            end
        end

        % Create files
        model_ppm = spm_create_vol(model_ppm);
        BMS.fname = fname;

        % Save data and BMS
        BMS.fname = fname;
        BMS.map.ffx.data = job.sess_map;
        save(out.files{1},'BMS', spm_get_defaults('mat.format'));

    case 'RFX',  % Random Effects

        % Check if BMS.mat exists
        if exist(fullfile(job.dir{1},'BMS.mat'),'file')
           load(fname);
           if  isfield(BMS,'map') && isfield(BMS.map,'rfx')
               str = { 'Warning: existing BMS.mat file has been over-written!'};
               msgbox(str)
           end
        end

        % BMS structure
        out.files{1}   = fname;

        % Create PPM image files for each model
        model_exp_r(1:nmodels) = struct(...
        'fname',    '',...
        'dim',      DIM',...
        'dt',       [spm_type('float32') spm_platform('bigend')],...
        'mat',      M,...
        'pinfo',    [1 0 0]',...
        'n', [1 1], ...
        'descrip',  '');

        if do_ecp
            % Create EPM image files for each model
            model_xp(1:nmodels) = struct(...
            'fname',    '',...
            'dim',      DIM',...
            'dt',       [spm_type('float32') spm_platform('bigend')],...
            'mat',      M,...
            'pinfo',    [1 0 0]',...
            'n', [1 1], ...
            'descrip',  '');
        end

        if do_alpha
            % Create alpha image files for each model
            model_alpha(1:nmodels) = struct(...
            'fname',    '',...
            'dim',      DIM',...
            'dt',       [spm_type('float32') spm_platform('bigend')],...
            'mat',      M,...
            'pinfo',    [1 0 0]',...
            'n', [1 1], ...
            'descrip',  '');
        end

        % Load Vols for all subjects/models
        for i = 1:nmodels
            model_exp_r(i).fname   = fullfile(direct,[sprintf('%s_model_xppm',names{i}) spm_file_ext]);
            model_exp_r(i).descrip = sprintf('Exp_r: %s model',names{i});
            BMS.map.rfx.ppm{i}     = model_exp_r(i).fname;
            if do_ecp
            model_xp(i).fname      = fullfile(direct,[sprintf('%s_model_epm',names{i}) spm_file_ext]);
            model_xp(i).descrip    = sprintf('XP: %s model',names{i});
            BMS.map.rfx.epm{i}     = model_xp(i).fname;
            end
            if do_alpha
            model_alpha(i).fname   = fullfile(direct,[sprintf('%s_model_alpha',names{i}) spm_file_ext]);
            model_alpha(i).descrip = sprintf('Alpha: %s model',names{i});
            BMS.map.rfx.alpha{i}   = model_alpha(i).fname;
            end
            for s = 1:nsubjs,
                for se = 1:nsess,
                    nsessi      = size(job.sess_map{s},2);
                    nmodelsi    = size(job.sess_map{s}(se).mod_map,1);
                    if (nsess == nsessi && nmodels == nmodelsi)
                        Vol_models(s,i,se) = spm_vol(job.sess_map{s}(se).mod_map(i));
                        tmp = Vol_models(s,i,se);
                    else
                        msgbox('The number of sessions/models should be the same for all subjects!')
                        return
                    end
                    % Stop if log-ev images have different dimensions
                    if tmp{1}.dim(1)~=xdim || tmp{1}.dim(2)~=ydim || tmp{1}.dim(3)~=zdim
                       error('Log-evidence images must have the same dimensions!')
                    end
                end
            end
        end

        % Create files
        model_exp_r              = spm_create_vol(model_exp_r);
        if do_ecp, model_xp      = spm_create_vol(model_xp); end
        if do_alpha, model_alpha = spm_create_vol(model_alpha); end

        % Save data and BMS
        BMS.fname = fname;
        BMS.map.rfx.data = job.sess_map;
        save(out.files{1},'BMS', spm_get_defaults('mat.format'));

end

% Progress bar
% -------------------------------------------------------------------------
spm_progress_bar('Init',zdim,'BMS Maps (Inference)','Slices complete');

% Loop through image slices
% -------------------------------------------------------------------------
for z = 1:zdim,

    spm_progress_bar('Set',z);                  % Update progress bar
    j = repmat(NaN,xdim,ydim);                  % Init. image values

    fprintf('%s%30s',repmat(sprintf('\b'),1,30),'Computing maps...')
    str   = sprintf('Slice %d out of %d',z,zdim); % Display slice nb.
    fprintf('\r%-40s: %30s',str,' ')

    zords   = z*zords_init;                     % Slice z
    xyz     = [xords(I); yords(I); zords(I)];   % Slice coordinates
    nVox    = size(xyz,2);                      % Nb. of voxels per slice

    if mask
        % Voxels inside mask
        mask_xyz  = mask_image{1}.mat\M*[xyz(:,1:nVox);ones(1,nVox)];
        gamma     = spm_get_data(mask_image{1},mask_xyz);
        b         = find(gamma>0.5);            % Voxels in the mask
    else
        b         = 1:nVox;                     % All voxels
    end

    z_models        = NaN(nsubjs,nmodels,nVox);       % Data
    z_models(1,1,:) = spm_get_data(first_vol{1},xyz); % Data: all subs/mods
    non_nan         = find(~isnan(z_models(1,1,:)));  % Voxels ~NaN

    % Find voxels ~NaN and sum sessions
    % ---------------------------------------------------------------------
    for s = 1:nsubjs,
        for k = 1:nmodels,
                sum_tmp_data    = [];
            for ns = 1:nsess,
                tmp_data        = Vol_models(s,k,ns);
                sum_tmp_data    = [sum_tmp_data; spm_get_data(tmp_data{1},xyz)];
            end
                z_models(s,k,:) = sum(sum_tmp_data,1);
                non_nani        = find(~isnan(z_models(s,k,:)));
                non_nan         = intersect(non_nan,non_nani);
        end
    end

    % Voxels to be analysed
    non_nan = intersect(non_nan,b);
    Nvoxels = length(non_nan);

    % Method
    % ---------------------------------------------------------------------
    switch method

          % Fixed Effects
          % ---------------------------------------------------------------
          case 'FFX',

              if Nvoxels > 0                % Slice with ~NaN voxels

                zz     = sum(z_models,1);   % Sum all subjects/sessions
                mz     = mean(zz,2);        % Get mean of all models
                zzmean = zeros(1,nmodels,length(mz));
                for jj = 1:nmodels
                    zzmean(1,jj,:) = mz;
                end

                if nmodels==1
                    % Process log Bayes factor image
                    zz  = exp(zz);              % Exponentiate log-bf values
                    % Calculate posterior probabiliy
                    pz  = zeros(1,1,length(zz));
                    pz  = zz./(1+zz);
                    j(non_nan)   = pz(1,1,non_nan);
                    model_ppm(1) = spm_write_plane(model_ppm(k),j,z);
                else
                    zz  = zz-zzmean;            % Subtract mean
                    zz  = exp(zz);              % Exponentiate log-ev values
                    tzz = sum(zz,2);            % Sum exp(log-ev.)

                    % Calculate posterior probabiliy
                    pz  = zeros(1,nmodels,length(zz));
                    for k = 1:nmodels,
                        pz(1,k,:)    = zz(1,k,:)./tzz;
                        j(non_nan)   = pz(1,k,non_nan);
                        model_ppm(k) = spm_write_plane(model_ppm(k),j,z);
                    end
                end

              else
                % Nvoxels = 0
                for k = 1:nmodels,
                    % Write NaN for slice z
                    model_ppm(k) = spm_write_plane(model_ppm(k),j,z);
                end
              end

          % Fixed Effects
          % ---------------------------------------------------------------
          case 'RFX',

                if Nvoxels > 0
                    % Initialise results
                    exp_r_total              = zeros(Nvoxels,nmodels);
                    if do_ecp, xp_total      = zeros(Nvoxels,nmodels); end
                    if do_alpha, alpha_total = zeros(Nvoxels,nmodels); end

                    % Do BMS in all voxels of slice z
                    for n = 1:Nvoxels,
                        lme = z_models(:,:,non_nan(n));

                        if nmodels==1
                            % Provide evidence for dummy null model
                            lme=0.5*[lme, -lme];
                            [alpha,exp_r,xp] = spm_BMS(lme,nsamps,0,0,do_ecp);

                            exp_r_total(n,:)              = exp_r(1);  % Cond. Expecta.
                            if do_ecp, xp_total(n,:)      = xp(1); end % Exceeda. Prob.
                            if do_alpha, alpha_total(n,:) = alpha(1); end % Dirichlet par.
                        else

                            % Group BMS
                            [alpha,exp_r,xp] = spm_BMS(lme,nsamps,0,0,do_ecp);

                            exp_r_total(n,:)              = exp_r;  % Cond. Expecta.
                            if do_ecp, xp_total(n,:)      = xp; end % Exceeda. Prob.
                            if do_alpha, alpha_total(n,:) = alpha; end % Dirichlet par.
                        end
                    end

                    % Write images
                    for i = 1:nmodels,
                        j(non_nan)     = exp_r_total(:,i);
                        model_exp_r(i) = spm_write_plane(model_exp_r(i),j,z);
                        if do_ecp
                        j(non_nan)     = xp_total(:,i);
                        model_xp(i)    = spm_write_plane(model_xp(i),j,z);
                        end
                        if do_alpha
                        j(non_nan)     = alpha_total(:,i);
                        model_alpha(i) = spm_write_plane(model_alpha(i),j,z);
                        end
                    end
                else
                    % Write images when Nvoxels = 0
                    for i = 1:nmodels,
                        model_exp_r(i) = spm_write_plane(model_exp_r(i),j,z);
                        if do_ecp, model_xp(i) = spm_write_plane(model_xp(i),j,z); end
                        if do_alpha, model_alpha(i) = spm_write_plane(model_alpha(i),j,z); end
                    end
                end

    end

end % Loop over slices

% Clear progress bar
% -------------------------------------------------------------------------
spm_progress_bar('Clear');
disp('Done.');


function mv = mv_exportSelectivity(mv, saveFlag, threshold, conditions);
%
% mv = mv_exportSelectivity(mv, [saveFlag], [threshold], conditions);
%
% Export, from a MultiVoxel UI to a mrVista view,
% a  parameter map of selectivity indices for each voxel,
% this map is saved as Selectivity-ROI.mat
% a corresponding colormap is save SelectivityCmap.mat
% which can be loaded from the Color Map UI
%
% Category indepdendent selecitivities range from 0-1
% The color indicates both the selectivity and category preference
% Thus, selectivity values are mapped to: category+selectivity
% where category are integers ranging from 0:Ncats-1
%
%  A second map is saved as a corAnal
%  note that this will overwrite the default corAnal performed on these data
%  This map is used for thresholding the selectivity map
%  co field contains voxel reliability (note that cothres has values of 0-1
%  thus, all negative reliabilities are going to be thresholded by default)
%  ph contains category independent selectivities
%  The co and ph fields can used to threshold the selectivity maps
%
% When loading a saved selectivity map need to load 3 files:
% File -> load Parmater Map-> Selectvity-ROIname
% File-> load CorAnal
% Color Map -> Parameter Map Mode -> Load colormap from file -> SelectivityCamp
%
% ras, 09/2005
% kgs 10/2005

if ieNotDefined('mv'), mv = get(gcf,'UserData'); end
if ~exist('threshold','var')
    threshold=0.1;
end
%%%%%check that a view exists
mrGlobals;
switch mv.roi.viewType
    case 'Inplane', view = getSelectedInplane;
    case 'Volume', view = getSelectedVolume;
    case 'Gray', view = getSelectedGray;
    case 'Flat', view = getSelectedFlat;
end
if isempty(view), error('No mrVista view opened.'); return; end

% get relevant params about the view
mapdims = viewGet(view,'dataSize');
nScans = viewGet(view,'numScans');
scan = mv.params.scans(1);

%%%%%get amplitudes for selected conditions
whichConds = find(tc_selectedConds(mv));
whichConds = whichConds(whichConds>1)-1; % remove null
nConds = length(whichConds);

%%%%%compute selectivity index
[scaledSel sel]= mv_selectivity(mv, whichConds, threshold);

% plug in the values to the map volume:
mapvol = zeros(mapdims);
ind = roiIndices(view,mv.coords);
mapvol(ind) = scaledSel;

%%%%%get voxel reliabilities, map to co map
if ~isfield(mv,'wta'), mv=mv_reliability(mv,'plotFlag',0); end
covol = zeros(mapdims);
covol(ind) = sel;
% % hack: move to range 0:1 instead of -1:1 so cothres can apply to the
% % whole range and not only positive numbers
% covol = 0.5*ones(mapdims);
% covol(ind) = mv.wta.voxR/2+covol(ind);

%%%%%map the reliabilityinto the ph map
phvol = zeros(mapdims);
% phvol(ind) = mod(sel, 1);
phvol(ind) = normalize(mv.wta.voxR, 0, 2*pi);

%%%%%create a color map
M = view.ui.mapMode;
colorsPerCond = floor(128/nConds);
M.numColors = colorsPerCond*nConds;
colors=[];
for i=1:nConds
    % make each condition have a gradient of colors up to
    % the full color specified by the color order
    col = mv.trials.condColors{whichConds(i)+1}; % full color
    for j = 1:colorsPerCond
        w = 0.3+ 0.5*(j-1)/colorsPerCond; % weight of color
        colors(end+1,:)=(w*col+ ((colorsPerCond-j+1)/(colorsPerCond))*[.7 .7 .7]);
    end
end
M.cmap = [gray(M.numGrays); colors];
max(M.cmap)
M.clipMode = [0.01 nConds+0.02]; % set to manual clip mode

% save the color map
cmapPath = fullfile(dataDir(view),'SelectivityCmap.mat');
cmap = M.cmap(M.numGrays+1:end,:);
save(cmapPath,'cmap');
fprintf('Saved color map in %s\n',cmapPath);

%%%%%set in view
map = cell(1,nScans); co = cell(1,nScans); ph = cell(1,nScans);
mapName = sprintf('Selectivity_%s',mv.roi.name);
map{scan} = mapvol; co{scan} = covol; ph{scan} = phvol;
view.map = map; view.mapName = mapName;
view.co = co; view.ph = ph; view.amp=map;
view.ui.mapMode = M;
try
    view=setParameterMap(view,map,mapName);
end
if saveFlag==1,
    saveParameterMap(view,[],1);
    saveCorAnal(view,1)
end
try
    refreshScreen(view);
catch
    return
end

% evaluate this in the workspace, so the view
% itself is updated
assignin('base','map',map);
assignin('base','co',co);
assignin('base','ph',ph);
assignin('base','tmp',M);

evalin('base',sprintf('%s=setParameterMap(%s,map,''%s'');',...
    view.name,view.name,mapName));
evalin('base',sprintf('%s=setDisplayMode(%s,''map'');',...
    view.name,view.name));
evalin('base',sprintf('%s.map = map;',view.name));
evalin('base',sprintf('%s.ui.mapMode=tmp;',view.name));
evalin('base',sprintf('%s.co=co;',view.name));
evalin('base',sprintf('%s.ph=ph;',view.name));
evalin('base',sprintf('%s=refreshScreen(%s);',view.name,view.name));

disp('Exported voxel selectivity to mrVista view.')

return


function [newnode,newelem,newface]=meshrefine(node,elem,varargin)
%
% [newnode,newelem,newface]=meshrefine(node,elem,face,opt)
%
% refine a tetrahedral mesh by adding new nodes or constraints
%
% author: Qianqian Fang (fangq<at> nmr.mgh.harvard.edu)
%
% input parameters:
%      node: existing tetrahedral mesh node list
%      elem: existing tetrahedral element list
%      face: (optional) existing tetrahedral mesh surface triangle list
%      opt:  options for mesh refinement:
%        if opt is a Nx3 array, opt is treated as a list of new nodes to
%          be inserted into the mesh (must be located on the surface or inside)
%        if opt is a struct, it can have the following fields:
%          opt.newnode: same as setting opt to an Nx3 array
%          opt.reratio: radius-edge ratio, by default, iso2mesh uses 1.414
%          opt.maxvol: maximum element volume
%
% outputs:
%      newnode: node coordinates of the tetrahedral mesh
%      newelem: element list of the tetrahedral mesh
%      newface: mesh surface element list of the tetrahedral mesh
%             the last column denotes the boundary ID
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

exesuff=getexeext;
exesuff=fallbackexeext(exesuff,'tetgen');

newpt=[];
opt=struct;
if(length(varargin)==1)
	face=[];
	if(isstruct(varargin{1}))
		opt=varargin{1};
    else
		newpt=varargin{1};
	end
elseif(length(varargin)>=2)
    face=varargin{1};
    if(isstruct(varargin{2}))
        opt=varargin{2};
    else
        newpt=varargin{2};
    end
else
	error('meshrefine requires at least 3 inputs');
end
if(isstruct(opt) && isfield(opt,'newnode'))
    newpt=opt.newnode;
end

% call tetgen to create volumetric mesh
deletemeshfile(mwpath('pre_refine.*'));
deletemeshfile(mwpath('post_refine.*'));

moreopt='';
setquality=0;
if(isstruct(opt) && isfield(opt,'reratio'))
	moreopt=[moreopt sprintf(' -q %.10f ',opt.reratio)];
	setquality=1;
end
if(isstruct(opt) && isfield(opt,'maxvol'))
    moreopt=[moreopt sprintf(' -a %.10f ',opt.maxvol)];
end

if(~isempty(newpt))
	savetetgennode(newpt,mwpath('pre_refine.1.a.node'));
	moreopt=' -i ';
end
if(size(elem,2)==3 && setquality==0)
    if(~isempty(newpt))
        error('inserting new point can not be used for surfaces');
    end
    nedge=savegts(node, elem,mwpath('pre_refine.gts'));
    exesuff=fallbackexeext(getexeext,'gtsrefine');
elseif(size(elem,2)==3)
    savesurfpoly(node,elem,[],[],[],[],mwpath('pre_refine.poly'));
else
    savetetgennode(node, mwpath('pre_refine.1.node'));
    savetetgenele (elem, mwpath('pre_refine.1.ele'));
end

fprintf(1,'refining the input mesh ...\n');

if(size(elem,2)==3 && setquality==0)
    if(isstruct(opt) && isfield(opt,'scale'))
        moreopt=sprintf('%s -n %d ',moreopt,round(nedge*opt.scale));
    else
        error('you must give opt.scale value for refining a surface');
    end
end
if(isstruct(opt) && isfield(opt,'moreopt'))
	moreopt=[moreopt opt.moreopt];
end

if(size(elem,2)==3 && setquality==0)
    system([' "' mcpath('gtsrefine') exesuff '" ' moreopt ' < "' ...
          mwpath('pre_refine.gts') '" > "' mwpath('post_refine.gts') '"']);
    [newnode,newelem]=readgts(mwpath('post_refine.gts'));
    newface=newelem;
elseif(size(elem,2)==3)
    system([' "' mcpath('tetgen') exesuff '" ' moreopt ' -p -A "' mwpath('pre_refine.poly') '"']);
    [newnode,newelem,newface]=readtetgen(mwpath('pre_refine.1'));
else
    system([' "' mcpath('tetgen') exesuff '" ' moreopt ' -r "' mwpath('pre_refine.1') '"']);
    [newnode,newelem,newface]=readtetgen(mwpath('pre_refine.2'));
end

% read in the generated mesh

fprintf(1,'mesh refinement is complete\n');


%Voice Based Biometric System
%By Ambavi K. Patel.

function ip2=preprocess(ip1,fs1,intval1)
[rw,cm]=size(ip1);
if rw==1
    ip1=ip1';
else ip1=ip1;
end;
ip1=ip1(:,1);                %to make one dimentional
len1=length(ip1);
i = 1;
while abs(ip1(i)) < 0.002 && i<48000% Silence detection
i = i + 1;
end
ip1(1:i) = [];
ip1=ip1-mean(ip1);
ip1=ip1*2;                 % amplification
len1=length(ip1);
i=fs1*intval1;
ip2=zeros(i,1);        % to make uniform dimention size
if len1<i
    ip2(1:len1)=ip1(1:len1);
else
    ip2(1:i)=ip1(1:i);
end;


function pass = test_plotting( pref )
% Check that the very basic plotting commands do not crash in diskfunv

F = grad(diskfun(@(x,y) x.*cos(y)));
try
    hold off
    quiver(F),         j = ishold;
    close all
    if ( j == 0 )
        pass(1) = 1;
    else
        pass(1) = 0;
    end
catch
    pass(1) = 0;
end

end


function []=panel6fdisp(N,n,T,Units,endo,Ymat,stringdates2,decimaldates2,Fstartlocation,Fendlocation,forecast_estimates,pref)

% preliminary task: reshape Ymat
Ymat=reshape(Ymat,T,n,N);
% preliminary task: gather in a cell the values to be plotted
% initiate the cell
plotdata={};
% because forecasts have to be computed for each unit, loop over units
for ii=1:N
% each cell entry is a matrix of actual and forecast values
% this matrix comprises 4 rows: the first row is actual data, while the three other rows are the estimates (point estimates and confidence bands) for the forecasts
% also, this matrix has a number of rows equal to the dimension of decimaldates2, which comprises the total period sample+forecasts
   % loop over variables
   for jj=1:n
      plotdata{jj,1,ii}=nan(4,size(decimaldates2,1));
      % record actual sample values
      plotdata{jj,1,ii}(1,1:T)=Ymat(:,jj,ii)';
      % copy the last point of the actual sample for the forecast part of the matrix (required to have a clean plot)
      plotdata{jj,1,ii}(:,Fstartlocation-1)=repmat(Ymat(Fstartlocation-1,jj,ii),4,1);
      % record forecast, lower bound
      plotdata{jj,1,ii}(2,Fstartlocation:Fendlocation)=forecast_estimates{jj,1,ii}(1,:);
      % record forecast, point estimate
      plotdata{jj,1,ii}(3,Fstartlocation:Fendlocation)=forecast_estimates{jj,1,ii}(2,:);
      % record forecast, upper bound
      plotdata{jj,1,ii}(4,Fstartlocation:Fendlocation)=forecast_estimates{jj,1,ii}(3,:);
   end
end
% then plot the figure
if pref.plot
forecast=figure('Tag','BEARresults');
set(forecast,'Color',[0.9 0.9 0.9]);
set(forecast,'name','unconditional forecasts');
% initiate the count
count=0;
% loop over units
for ii=1:N
   % loop over endogenous variables
   for jj=1:n
   % increment count
   count=count+1;
   % then plot
   subplot(N,n,count)
   hold on
   Xpatch=[decimaldates2(Fstartlocation-1:Fendlocation,1)' fliplr((decimaldates2(Fstartlocation-1:Fendlocation,1))')];
   Ypatch=[plotdata{jj,1,ii}(2,Fstartlocation-1:Fendlocation) fliplr(plotdata{jj,1,ii}(4,Fstartlocation-1:Fendlocation))];
   Fpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
   set(Fpatch,'facealpha',0.6);
   set(Fpatch,'edgecolor','none');
   plot(decimaldates2,plotdata{jj,1,ii}(3,:),'Color',[0.4 0.4 1],'LineWidth',2);
   plot(decimaldates2,plotdata{jj,1,ii}(1,:),'Color',[0 0 0],'LineWidth',2);
   hold off
   set(gca,'XLim',[decimaldates2(1,1) decimaldates2(end,1)],'FontName','Times New Roman');
   set(gca,'XGrid','on');
   set(gca,'YGrid','on');
      % top labels
      if count<=n
      title(endo{count,1},'FontWeight','normal');
      end
      % side labels
      if jj==1
      ylabel(Units{ii,1},'FontWeight','normal');
      end
   end
end
end

% save on Excel
% create the cell that will be saved on excel
forecastcell={};
% build preliminary elements: space between the tables
vertspace=repmat({''},size(stringdates2,1)+3,1);
horzspace=repmat({''},3,6*n);
% loop over units
for ii=1:N
% initiate the cell of results
unitcell={};
   % loop over endogenous variables (horizontal dimension)
   for jj=1:n
   % create a header
   header=[{[Units{ii,1} ': ' endo{jj,1}]} {''} {''} {''} {''};{''} {''} {''} {''} {''};{''} {'actual'} {'lower bound'} {'median'} {'upper bound'}];
   % complete the cell
   endocell=[[header;stringdates2 num2cell((plotdata{jj,1,ii})')] vertspace];
   % concatenate to the previous parts of unitcell
   unitcell=[unitcell endocell];
   end
% concatenate to the previous parts of afcell
forecastcell=[forecastcell;horzspace;unitcell];
end
% trim
forecastcell=forecastcell(4:end,1:end-1);
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),forecastcell,'forecasts','B2');
end


function plotIPDF(SO3F,r,varargin)
% plot inverse pole figures
%
% Input
%  odf - @SO3FunHarmonic
%  r   - @vector3d specimen directions
%
% Options
%  RESOLUTION - resolution of the plots
%
% Flags
%  antipodal - include <VectorsAxes.html antipodal symmetry>
%  complete  - plot entire (hemi)--sphere
%
% See also
% S2Grid/plot savefigure Plotting Annotations_demo ColorCoding_demo PlotTypes_demo
% SphericalProjection_demo

if numel(SO3F)>1
  warning(['You try to plot an multivariate function. Plot the desired components ' ...
    'manually. In the following the first component is plotted.'])
end

plotIPDF@SO3Fun(SO3F.subSet(1),r,varargin{:});

end


classdef (Abstract) FilterX < BaseX % Extends trackingX.BaseX
% FilterX Abstract class
%
% Summary of FilterX:
% This is the base class for all TrackingX filters.
% Any custom defined Filter should be derived from this FilterX base class.
%
% By default, FilterX objects are designed with the intention to be utilised
% as (Markov-Chain) State Estimators that opererate on a given problem as
% deterministic Finite State Machines (FSMs), with self-contained memory.
% In other words, it is expected that for most problems, FilterX objects will
% go through any heavy parameterisation once, after which they will be used
% to recursively execute a Prediction-Update State Estimation loop.
% FilterX objects aim to preserve all the inter-state information they require
% such that they can execute the next FSM step, without being provided with
% information they already know.
%
%
% FilterX Properties:
%   + Model      - Object handle to StateSpaceModelX class, containing model
%                  parameterisations. Normally set once during filter initialisation and
%                  accessed later on.
%   + Prior      - Structure containing prior information (e.g. pdf)
%                  Normally set once during filter initialisation and
%                  accessed later on.
%   + Prediction - Structure containing prediction information (e.g. pdf)
%                  Data is over-written on every prediction step and is
%                  subsequently read by the update (and later) step(s)
%   + Posterior  - Structure containing posterior information (e.g. pdf)
%                  Data is over-written on every update step and is subsequently
%                  utilised by
%   + MeasurementList   - A (matrix of) column vector(s) representing the
%                         measurement(s) to be used during the filter update step
%
%   (*) Signifies properties necessary to instantiate a class object
%
% FilterX Methods:
%   + FilterX    - Constructor method
%   + predict    - Performs filter prediction step
%   + update     - Performs filter update/correction step
%
% (+) denotes puplic properties/methods
%
% See also TransitionModelX, MeasurementModelX and ControlModelX
%
% November 2018 Lyudmil Vladimirov, University of Liverpool.

    properties
        Model
        MeasurementList
    end

    properties (Access = protected)

    end

    methods (Abstract)
        predict(this);
        update(this);
    end

    methods (Access = protected)
        function initialise_(this, config)
            if (isfield(config,'Model'))
                this.Model = config.Model;
            end
        end
    end

    methods
        function this = FilterX(varargin)
        % FilterX Constructor method
        %
        % Parameters
        % ----------
        % Model: StateSpaceModelX
        %  Object handle to a given state-space model
        %
        % Usage
        % -----
        % * FilterX(__, Name, Value) instantiates an object handle, configured
        %   the parameters specified by one or more Name,Value pair arguments.
        % * FilterX(config) instantiates an object handle configured with
        %   the parameters specified inside the 'config' structure, whose
        %   fieldnames correspond to the parameter names as given in the
        %   Parameters section above.
        %
        %  See also predict, update, iterate, smooth.

            if(nargin==0)
                return;
            end

            % First check to see if a structure was received
            if(nargin==1)
                if(isstruct(varargin{1}))
                    this.initialise_(varargin{1});
                    return;
                end
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            this.initialise_(parser.Unmatched);
        end

        function initialise(this, varargin)
        % initialise Initialisation method. Reset and re-configure the filter.
        %
        % Parameters
        % ----------
        % Model: StateSpaceModelX
        %  Object handle to a given state-space model.
        %
        % Usage
        % -----
        % * filter.initialise(__, Name, Value) instantiates an object handle,
        %   configured the parameters specified by one or more Name,Value
        %   pair arguments.
        % * filter.initialise(config) instantiates an object handle configured
        %   with the parameters specified inside the 'config' structure, whose
        %   fieldnames correspond to the parameter names as given in the
        %   Parameters section above.
        %
        %  See also predict, update, iterate, smooth.

            if(nargin==1)
                return;
            end

            % First check to see if a structure was received
            if(nargin==2)
                if(isstruct(varargin{1}))
                    this.initialise_(varargin{1});
                    return;
                end
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            this.initialise_(parser.Unmatched);
        end

        % ===============================>
        % ACCESS METHODS
        % ===============================>
        function set.Model(this,newModel)
            this.Model = setModel(this,newModel);
        end

        function set.MeasurementList(this,newMeasurementList)
            this.MeasurementList = setMeasurementList(this,newMeasurementList);
        end
    end

    methods (Access = protected)
        % ===============================>
        % ACCESS METHOD HANDLES
        % ===============================>
        function Prior = setPrior(this,newPrior)
            Prior = newPrior;
        end
        function Prediction = setPrediction(this,newPrediction)
            Prediction = newPrediction;
        end
        function Posterior = setPosterior(this,newPosterior)
            Posterior = newPosterior;
        end
        function Model = setModel(this,newModel)
            Model = newModel;
        end
        function measurementList = setMeasurementList(this,newMeasurementList)
            measurementList = newMeasurementList;
        end
    end
end


function TestPhoneRecognizerLSTM_TIMIT()

modeldir = my_dir('nnet');
modelfiles = findFiles(['nnet/' modeldir{1}], 'mat');
modelfiles = sort_nnet_by_itr(modelfiles);      % get the last iteration model

dnn = load(modelfiles{1});
para = dnn.para;
para.IO = RemoveIOStream(para.IO, 2);   % during evaluation, we don't have the label usually. So remove that stream from the configuration
layer = dnn.layer(1:end-2);     % we discard the last two layers that is not useful for ASR

clean_cond = 1;
[Data_cv, ~, para] = LoadData_TIMIT(para, 'test');

para.out_layer_idx = length(layer) + [0];   % you can specify which layers' activation will be outputed

for i=1:length(Data_cv(1).data)
    tmpData(1).data{1} = Data_cv(1).data{i};
    output = FeatureTree2(tmpData, para, layer);
    posterior = output{1}{1};

    subplot(2,1,1); imagesc(Data_cv(1).data{i});    title('Feature');
    subplot(2,1,2); imagesc(posterior);       title('Posteriorgram and true label'); hold on
    plot(Data_cv(2).data{i}, 'r'); hold off;
    pause
end

end


function PlotImu6(aedat, numBins, startTime, endTime)

%{
%}

if isfield(aedat.data, 'imu6')
    data = aedat.data.imu6;
else
    return
end

if ~exist('numBins', 'var') || (exist('numBins', 'var') && numBins == 0)
	numBins = data.numEvents;
end

if exist('startTime', 'var') && startTime > 0
    startTime = startTime * 1e6;
else
    startTime = aedat.info.firstTimeStamp;
end

if exist('endTime', 'var') && endTime > 0
    endTime = endTime * 1e6;
else
    endTime = aedat.info.lastTimeStamp;
end

durationUs = double(endTime - startTime);
durationOfBinUs = durationUs / numBins;
durationOfBinS = durationOfBinUs / 1000000;

timeBinBoundariesUs = double(startTime) : durationOfBinUs : double(endTime);
timeBinCentresS = (timeBinBoundariesUs(1 : end - 1) + durationOfBinUs / 2) / 1000000;

if numBins == data.numEvents
    accelX = data.accelX;
    accelY = data.accelY;
    accelZ = data.accelZ;
    gyroX  = data.gyroX;
    gyroY  = data.gyroY;
    gyroZ  = data.gyroZ;
    temperature   = data.temperature;
else
    accelX = zeros(numBins, 1);
    accelY = zeros(numBins, 1);
    accelZ = zeros(numBins, 1);
    gyroX  = zeros(numBins, 1);
    gyroY  = zeros(numBins, 1);
    gyroZ  = zeros(numBins, 1);
    temperature = zeros(numBins, 1);

    for bin = 1 : numBins
		firstTimeStampIndex = find(data.timeStamp >= timeBinBoundariesUs(bin), 1, 'first');
		lastTimeStampIndex = max(firstTimeStampIndex, find(data.timeStamp < timeBinBoundariesUs(bin + 1), 1, 'last'));
		if ~isempty(firstTimeStampIndex) && ~isempty(lastTimeStampIndex)
			accelX(bin) = mean(data.accelX(firstTimeStampIndex : lastTimeStampIndex));
			accelY(bin) = mean(data.accelZ(firstTimeStampIndex : lastTimeStampIndex));
			accelZ(bin) = mean(data.accelY(firstTimeStampIndex : lastTimeStampIndex));
			gyroX(bin) = mean(data.gyroX(firstTimeStampIndex : lastTimeStampIndex));
			gyroY(bin) = mean(data.gyroY(firstTimeStampIndex : lastTimeStampIndex));
			gyroZ(bin) = mean(data.gyroZ(firstTimeStampIndex : lastTimeStampIndex));
			temperature(bin) = mean(data.temperature(firstTimeStampIndex : lastTimeStampIndex));
		end
    end
end

figure
legendLocal = {};
hold all
plot(timeBinCentresS, accelX, '-')
legendLocal = [legendLocal 'accelX'];
plot(timeBinCentresS, accelY, '-')
legendLocal = [legendLocal 'accelY'];
plot(timeBinCentresS, accelZ, '-')
legendLocal = [legendLocal 'accelZ'];
xlabel('Time (s)')
ylabel('Acceleration (g)')
legend(legendLocal)

figure
legendLocal = {};
hold all
plot(timeBinCentresS, gyroX)
legendLocal = [legendLocal 'gyroX'];
plot(timeBinCentresS, gyroY)
legendLocal = [legendLocal 'gyroY'];
plot(timeBinCentresS, gyroZ)
legendLocal = [legendLocal 'gyroZ'];
xlabel('Time (s)')
ylabel('Angular velocity (deg/s)')
legend(legendLocal)

figure
plot(timeBinCentresS, temperature)
xlabel('Time (s)')
ylabel('temperature (C)')


 function y = mtimes(ob, x)
%function y = mtimes(ob, x)
%	y = G * x	or x = G' * y
%	Copyright 2002-2-20	Jeff Fessler	The University of Michigan

%
%	scalar * G
%
if isa(ob, 'double') & length(x) == 1 & isa(x, 'Gtomo2_dsc')
	y = x;
	y.scale = ob;
	return
end

if ob.apower ~= 1, error notdone, end

%
%	partial projection or backprojection (for ordered subsets)
%
if ob.is_subset
	%
	%	Gt(:,ii)' * x		partial forward projection
	%
	if ~ob.is_transpose
		if ob.is_masked
			x = embed(x, ob.mask);
		end
		y = wtfmex('dsc,proj', ob.arg', single(x), ob.mask, ...
			int32(ob.ia_start), int32(ob.ia_inc), ...
			int32(ob.nthread), ob.chat);
		y = y(:,(ob.ia_start+1):ob.ia_inc:ob.na);

	%
	%	Gt(:,ii) * y		partial back projection
	%
	else
		y = zeros(ob.nb,ob.na);
		ia = (ob.ia_start+1):ob.ia_inc:ob.na;
		y(:,ia) = reshape(x, ob.nb, length(ia));
		y = wtfmex('dsc,back', ob.arg', single(y), ob.mask, ...
			int32(ob.ia_start), int32(ob.ia_inc), ...
			int32(ob.nthread), ob.chat);
		if ob.is_masked
			y = y(ob.mask);
		end
	end

%
%	full projection
%
elseif ~ob.is_transpose
	if ob.is_masked
		x = embed(x, ob.mask);
	end
	y = wtfmex('dsc,proj', ob.arg', single(x), ob.mask, ...
		int32(0), int32(1), int32(ob.nthread), ob.chat);

%
%	full back-projection
%
else
	y = wtfmex('dsc,back', ob.arg', single(x), ob.mask, ...
		int32(0), int32(1), int32(ob.nthread), ob.chat);
	if ob.is_masked
		y = y(ob.mask);
	end
end

y = ob.scale * double(y(:));