function nbrhood=cosmo_spherical_neighborhood(ds, varargin)
% computes neighbors for a spherical searchlight
%
% nbrhood=cosmo_spherical_neighborhood(ds, opt)
%
% Inputs
%   ds                  a dataset struct, either:
%                       - in fmri form (from cosmo_fmri_dataset), when
%                         ds.fa has the fields .i, .j and .k
%                       - in meeg source form (from cosmo_meeg_dataset),
%                         when ds.fa has the field .pos. In this case, the
%                         features must have positions that can be
%                         converted to a grid.
%   'radius', r         } either use a radius of r, or select
%   'count', c          } approximately c voxels per searchlight
%                       Notes:
%                       - These two options are mutually exclusive
%                       - When using this option for an fmri dataset, the
%                         radius r is expressed in voxel units; for an meeg
%                         source dataset, the radius r is in whatever units
%                         the source dataset uses for the positions
%   'progress', p       show progress every p features (default: 1000)
%
% Outputs
%   nbrhood             dataset-like struct without .sa or .samples, with:
%     .a                dataset attributes, from dataset.a
%     .fa               feature attributes with the same fields as fs.fa,
%                       and in addition the fields:
%       .nvoxels        1xP number of voxels in each searchlight
%       .radius         1xP radius in voxel units
%       .center_ids     1xP feature center id
%     .neighbors        Px1 cell so that center2neighbors{k}==nbrs contains
%                       the feature ids of the neighbors of feature k
%                       If the dataset has a field ds.fa.inside, then
%                       features that are not inside are not included as
%                       neighbors in the output
%     .origin           Has fields .a and .fa from input dataset
%
%
% Example:
%     ds=cosmo_synthetic_dataset('type','fmri');
%     radius=1; % radius=3 is typical for 'real-world' searchlights
%     nbrhood=cosmo_spherical_neighborhood(ds,'radius',radius,...
%                                             'progress',false);
%     cosmo_disp(nbrhood)
%     %|| .a
%     %||   .fdim
%     %||     .labels
%     %||       { 'i'  'j'  'k' }
%     %||     .values
%     %||       { [ 1         2         3 ]  [ 1         2 ]  [ 1 ] }
%     %||   .vol
%     %||     .mat
%     %||       [ 2         0         0        -3
%     %||         0         2         0        -3
%     %||         0         0         2        -3
%     %||         0         0         0         1 ]
%     %||     .dim
%     %||       [ 3         2         1 ]
%     %||     .xform
%     %||       'scanner_anat'
%     %|| .fa
%     %||   .nvoxels
%     %||     [ 3         4         3         3         4         3 ]
%     %||   .radius
%     %||     [ 1         1         1         1         1         1 ]
%     %||   .center_ids
%     %||     [ 1         2         3         4         5         6 ]
%     %||   .i
%     %||     [ 1         2         3         1         2         3 ]
%     %||   .j
%     %||     [ 1         1         1         2         2         2 ]
%     %||   .k
%     %||     [ 1         1         1         1         1         1 ]
%     %|| .neighbors
%     %||   { [ 1         4         2 ]
%     %||     [ 2         1         5         3 ]
%     %||     [ 3         2         6 ]
%     %||     [ 4         1         5 ]
%     %||     [ 5         4         2         6 ]
%     %||     [ 6         5         3 ]           }
%     %|| .origin
%     %||   .a
%     %||     .fdim
%     %||       .labels
%     %||         { 'i'
%     %||           'j'
%     %||           'k' }
%     %||       .values
%     %||         { [ 1         2         3 ]
%     %||           [ 1         2 ]
%     %||           [ 1 ]                     }
%     %||     .vol
%     %||       .mat
%     %||         [ 2         0         0        -3
%     %||           0         2         0        -3
%     %||           0         0         2        -3
%     %||           0         0         0         1 ]
%     %||       .dim
%     %||         [ 3         2         1 ]
%     %||       .xform
%     %||         'scanner_anat'
%     %||   .fa
%     %||     .i
%     %||       [ 1         2         3         1         2         3 ]
%     %||     .j
%     %||       [ 1         1         1         2         2         2 ]
%     %||     .k
%     %||       [ 1         1         1         1         1         1 ]
%
%
% Notes:
%   - this function can return neighborhoods with either a fixed number of
%     features, or a fixed radius. When used with a searchlight, the
%     former has the advantage that the number of features is less
%     variable (especially near edges of the brain, in an fmri dataset),
%     which can make it easier to compare result in different regions as
%     the number of features can affect
%     pattern discriminablity. The latter has the advantage that the
%     smoothness of the output maps under the null hypothesis can be more
%     uniformly smooth.
%
% See also: cosmo_fmri_dataset, cosmo_meeg_dataset, cosmo_searchlight
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    check_input(varargin{:});

    defaults=struct();
    defaults.progress=1000;
    opt=cosmo_structjoin(defaults,varargin);

    [use_fixed_radius,radius,voxel_count]=get_selection_params(opt);
    cosmo_check_dataset(ds);

    % ensure not too many features are requested
    feature_mask=get_features_mask(ds);
    nfeatures=sum(feature_mask);
    if nfeatures<voxel_count
        error('Cannot select %d features: only %d are present',...
                    voxel_count, nfeatures);
    end

    % get attributes for output dataset, and the positions and dimension of
    % the grid
    [fdim,fa,pos,grid_dim]=get_spherical_attributes(ds,feature_mask);

    % compute voxel offsets relative to origin
    [sphere_offsets, distances]=get_sphere_offsets(radius);

    % get mapping from linear ids to feature ids
    [lin2feature_ids,lin2feature_mask]=get_lin2feature_ids(grid_dim,pos...
                                        ,feature_mask);

    % number of features associated with each linear id
    feature_id_count=sum(lin2feature_mask,2);

    show_progress=opt.progress>0;

    if show_progress
        clock_start=clock();
        prev_progress_msg='';
    end

    % a position may occur at multiple features; only consider unique
    % positions
    pos(:,~feature_mask)=Inf;
    [center_idxs,unq_pos]=cosmo_index_unique(pos');
    keep_unq_pos=~any(isinf(unq_pos),2);
    center_idxs=center_idxs(keep_unq_pos);
    unq_pos=unq_pos(keep_unq_pos,:);
    nunq_centers=numel(center_idxs);

    % allocate space for output
    ncenters=nunq_centers;
    neighbors=cell(ncenters,1);
    nvoxels=zeros(1,ncenters);
    final_radius=zeros(1,ncenters);
    visited=false(1,ncenters);
    center_ids=zeros(1,ncenters);

    % go over all features
    for k=1:nunq_centers
        variable_radius=NaN;
        if voxel_count==0
            feature_ids=zeros(1,0);
        else
            center_pos=unq_pos(k,:)';

            % - in case of a variable radius, keep growing sphere_offsets
            %   until there are enough voxels selected. This new radius is
            %   kept for every subsequent iteration.
            % - in case of a fixed radius this loop is left after the first
            %   iteration.
            while true
                % add offsets to center
                all_around_pos=bsxfun(@plus, center_pos', sphere_offsets);

                % see which ones are outside the volume
                outside_msk=all_around_pos<=0 | ...
                                bsxfun(@minus,grid_dim,all_around_pos)<0;

                % collapse over 3 dimensions
                feature_outside_msk=any(outside_msk,2);

                % get rid of those outside the volume
                around_pos=all_around_pos(~feature_outside_msk,:);

                % convert to linear indices
                around_lin=fast_sub2ind(grid_dim,around_pos(:,1), ...
                                            around_pos(:,2), ...
                                            around_pos(:,3));

                % convert linear to feature ids
                % (transpose is necessary so that when applying the
                %  mask, the indices remain sorted by distance)
                around_ids_mat=lin2feature_ids(around_lin,:)';
                around_ids_mask=lin2feature_mask(around_lin,:)';
                feature_ids=around_ids_mat(around_ids_mask);

                if use_fixed_radius
                    break; % we're done selecting voxels
                elseif numel(feature_ids)<voxel_count
                    % the current radius is too small.
                    % increase the radius by half a voxel and recompute new
                    % offsets, then try again in the next iteration.
                    radius=radius+.5;
                    [sphere_offsets,distances]=get_sphere_offsets(radius);
                    continue
                end

                % if using variable radius, compute distance of each linear
                % index
                center_distances=distances(~feature_outside_msk);

                % get distance for each feature
                feature_distances=get_distances(center_distances,...
                                            feature_id_count(around_lin));

                % coming here, the radius is variable and enough features
                % were selected. Now decide which voxels to keep,
                % and also compute the metric radius, then leave the while
                % loop.

                nselect=boundary_at_approx(feature_ids,...
                                            feature_distances,voxel_count);
                feature_ids=feature_ids(1:nselect);

                variable_radius=feature_distances(nselect);
                break; % we're done
            end
        end

        % store results
        id=center_idxs{k}(1);

        neighbors{k}=feature_ids(:)';
        nvoxels(k)=numel(feature_ids);
        if use_fixed_radius
            final_radius(k)=radius;
        else
            final_radius(k)=variable_radius;
        end

        visited(k)=true;
        assert(center_ids(k)==0);
        center_ids(k)=id;

        if show_progress && (k==1 || k==nunq_centers || ...
                                        mod(k,opt.progress)==0)
            mean_size=mean(nvoxels(visited));
            msg=sprintf('mean size %.1f', mean_size);
            prev_progress_msg=cosmo_show_progress(clock_start, ...
                                   k/nunq_centers, msg, prev_progress_msg);
        end
    end

    not_visited_ids=find(~visited);
    assert(all(cellfun(@numel,neighbors(not_visited_ids))==0));
    neighbors(not_visited_ids)=repmat({zeros(1,0)},...
                                        1,numel(not_visited_ids));

    % set the dataset and feature attributes
    nbrhood=struct();
    nbrhood.a=ds.a;
    nbrhood.a.fdim=fdim;

    % remove sample dimension if present
    if isfield(nbrhood.a,'sdim')
        nbrhood.a=rmfield(nbrhood.a,'sdim');
    end

    fa_full=cosmo_slice(fa,center_ids,2,'struct');
    nbrhood.fa=cosmo_structjoin('nvoxels',nvoxels,...
                                'radius',final_radius,...
                                'center_ids',center_ids(:)',...
                                fa_full);

    nbrhood.neighbors=neighbors;

    nbrhood=align_nbrhood_to_ds_if_possible(ds,nbrhood);
    origin=struct();
    origin.a=ds.a;
    origin.fa=ds.fa;
    nbrhood.origin=origin;

    cosmo_check_neighborhood(nbrhood,ds);

function nbrhood=align_nbrhood_to_ds_if_possible(ds,nbrhood)
   labels=get_dim_label(ds);

   ds_fa=get_spherical_fa_cell(ds.fa,labels);
   nbrhood_fa=get_spherical_fa_cell(nbrhood.fa,labels);

   [unq_ds,idx_ds]=cosmo_index_unique(ds_fa);
   [unq_nbrhood,idx_nbrhood]=cosmo_index_unique(nbrhood_fa);

   if all(cellfun(@numel,unq_ds)==1) && ...
           isequal(sort(cell2mat(unq_ds)),sort(cell2mat(unq_nbrhood)))
       mp=cosmo_align(nbrhood_fa,ds_fa);

       nbrhood.neighbors=nbrhood.neighbors(mp);
       nbrhood.fa=cosmo_slice(nbrhood.fa,mp,2,'struct');
   end

function feature_distances=get_distances(center_distances,feature_id_count)
    % get distances based on selected features
    n=numel(center_distances);
    assert(n==numel(feature_id_count));

    m=max(feature_id_count);
    if m<=1
        % optimization
        feature_distances=center_distances(feature_id_count==1);
        return
    end

    ds=NaN(m,n);

    for k=1:m
        msk=feature_id_count>=k;
        ds(k,msk)=center_distances(msk);
    end

    feature_distances=ds(~isnan(ds));

function [lin2feature_ids,lin2feature_mask]=get_lin2feature_ids(...
                                            grid_dim,all_pos,center_mask)
    % returns a function that maps linear ids to feature ids
    % the function takes as input linear ids and the distance for each
    % linear id, and returns the feature ids and their corresponding
    % distances

    orig_nvoxels=prod(grid_dim);

    ijk=all_pos(:,center_mask);

    lin_ids=fast_sub2ind(grid_dim, ijk(1,:), ijk(2,:), ijk(3,:));
    [idxs,unq_lin_ids]=cosmo_index_unique(lin_ids');

    mask2full=find(center_mask);
    % lin2feature_ids{k}={i1,...,iN} means that the linear voxel index k
    % corresponds to features i1,...iN
    lin2feature_ids_cell=cell(orig_nvoxels,1);
    for k=1:numel(unq_lin_ids)
        lin_id=unq_lin_ids(k);
        idx=idxs{k}(:)';
        lin2feature_ids_cell{lin_id}=mask2full(idx);
    end

    n_max=max(cellfun(@numel,lin2feature_ids_cell));

    lin2feature_ids=zeros(orig_nvoxels, n_max);
    lin2feature_mask=false(orig_nvoxels,n_max);

    for k=1:numel(unq_lin_ids)
        lin_id=unq_lin_ids(k);
        indices=lin2feature_ids_cell{lin_id};

        cols=1:numel(indices);
        lin2feature_ids(lin_id,cols)=indices;
        lin2feature_mask(lin_id,cols)=true;
    end

function feature_mask=get_features_mask(ds)
    % use .fa.inside if it is present, otherwise an array with only true
    % values
    nfeatures=size(ds.samples,2);

    if cosmo_isfield(ds,'fa.inside')
        inside=ds.fa.inside;

        if size(inside,1)~=1
            error('field .fa.inside must be a row vector');
        end

        if ~islogical(inside)
            error('field .fa.inside must be logical');
        end

        feature_mask=inside;
    else
        feature_mask=true(1,nfeatures);
    end

function lin=fast_sub2ind(sz, i, j, k)
    lin=sz(1)*(sz(2)*(k-1)+(j-1))+i;

function pos=boundary_at_approx(ids, distances, voxel_count)
    % pseudo-random selection of approximatly voxel_count elements
    if voxel_count<=0
        pos=0;
        return
    end

    assert(issorted(distances));

    max_distance=distances(voxel_count);
    first=find(distances<max_distance,1,'last')+1;
    last=find(distances>max_distance,1,'first')-1;

    if isempty(first)
        first=1;
    end

    if isempty(last)
        last=numel(distances);
    end

    delta_first=voxel_count-first;
    delta_last=last-voxel_count;

    if delta_first==delta_last
        % select pseudo-randomly
        if delta_first==0 || mod(sum(ids)+numel(distances),2)==0
            pos=first;
        else
            pos=last;
        end
    elseif delta_first<delta_last
        pos=first;
    else
        pos=last;
    end

    assert(first==1 || distances(first-1)<distances(first));
    assert(last==numel(distances) || distances(last+1)>distances(first));

function [fdim,fa,ijk,orig_dim]=get_spherical_attributes(ds, center_mask)
    % returns fdim, fa, and ijk positions for dataset
    labels=get_dim_label(ds);

    fdim=get_spherical_fdim(ds,labels);
    fa=get_spherical_fa(ds.fa,labels);

    if cosmo_isfield(ds,'fa.inside')
        fa.inside=center_mask;
    end

    small_ds=cosmo_slice(ds,[],1);
    small_ds_vol=cosmo_vol_grid_convert(small_ds, 'tovol');

    ijk=[small_ds_vol.fa.i;small_ds_vol.fa.j;small_ds_vol.fa.k];

    ijk_labels={'i','j','k'};
    [unused,index]=has_fdim_label(small_ds_vol,ijk_labels);

    orig_dim=cellfun(@numel,small_ds_vol.a.fdim.values(index));
    orig_dim=orig_dim(:)';

function [tf,index]=has_fdim_label(ds, label)
    [two,index]=cosmo_dim_find(ds,label,false);
    tf=~isempty(two) && two==2;

function [labels,index]=get_dim_label(ds)
    % get either pos or i, j, and k labels
    possible_labels={{'pos'},{'i';'j';'k'}};
    for j=1:numel(possible_labels)
        labels=possible_labels{j};
        [has_label,index]=has_fdim_label(ds, labels);
        if has_label
            return
        end
    end

    error(['Unable to find dimension labels, either ''pos'' '...
                    'or ''i'', ''j'', and ''k''']);

function fdim=get_spherical_fdim(ds, target_labels)
    first_target_label=target_labels{1};
    [two, index]=cosmo_dim_find(ds,first_target_label,true);

    if two~=2
        error('dimension ''%s'' must be a feature dimension');
    end
    cosmo_isfield(ds,'a.fdim.labels',true);

    dim_labels=ds.a.fdim.labels(:);
    dim_values=ds.a.fdim.values(:);

    nlabels=numel(target_labels);
    idx_labels=(index+(0:(nlabels-1)))';
    if numel(dim_labels)<index+(nlabels-1) || ...
            ~isequal(dim_labels(idx_labels),target_labels)
        error('expected labels %s in .a.fdim.labels(%d:%d)',...
                  cosmo_strjoin(target_labels,', '),...
                idx_labels(1), idx_labels(end));
    end

    fdim=struct();
    fdim.labels=dim_labels(idx_labels);
    fdim.values=dim_values(idx_labels);

    fdim=ensure_row_vector_or_3d_matrix(fdim);

function fdim=ensure_row_vector_or_3d_matrix(fdim)
    labels=fdim.labels;
    nlabels=numel(labels);

    keys={'labels','values'};
    nkeys=numel(keys);
    for k=1:nlabels
        label=labels{k};
        for j=1:nkeys
            key=keys{j};
            value=fdim.(key){k};

            if strcmp(label,'pos') && strcmp(key,'values')
                if size(value,1)~=3
                    error(['''pos'' attribute in .a.fdim.values '...
                                'must be 3xM']);
                end
            else
                if ~isvector(value)
                    error(['''%s'' attribute in .a.fdim.%s must '...
                            'be a vector'],labels,key);
                end
                fdim.(key){k}=value(:)';
            end
        end
    end

function fa=get_spherical_fa(ds_fa, target_labels)
    fa_cell=get_spherical_fa_cell(ds_fa, target_labels);
    fa=cell2struct(fa_cell,target_labels,2);

function fa_cell=get_spherical_fa_cell(ds_fa, target_labels)
    nlabels=numel(target_labels);
    fa_cell=cell(1,nlabels);
    for j=1:nlabels
        label=target_labels{j};
        fa_cell{j}=ds_fa.(label);
    end

function [sphere_offsets, o_distances]=get_sphere_offsets(radius)
    % return offsets and euclidean (and a bit manhattan) distance
    % from origin
    [sphere_offsets, norm2_distances]=cosmo_sphere_offsets(radius);

    % compute manhattan distance
    norm1_distances=sum(abs(sphere_offsets),2);

    % add a tiny bit of manhattan to make distances more varied
    norm12_distances=norm2_distances+1e-5*norm1_distances;

    % ensure distances are sorted
    [o_distances,i]=sort(norm12_distances);
    sphere_offsets=sphere_offsets(i,:);

function check_input(varargin)
    if numel(varargin)<1 || isscalar(varargin{1})
        % change in parameters
        raise_parameter_error();
    end

function [use_fixed_radius,radius,voxel_count]=get_selection_params(opt)
    if isfield(opt,'radius')
        if isfield(opt,'count')
            raise_parameter_error();
        elseif isscalar(opt.radius) && opt.radius>=0
            use_fixed_radius=true;
            radius=opt.radius;
            voxel_count=NaN;
            return
        end
    elseif isfield(opt,'count') && isscalar(opt.count) && ...
                opt.count>=0 && round(opt.count)==opt.count
        use_fixed_radius=false;
        radius=1; % starting point
        voxel_count=opt.count;
        return;
    end

    raise_parameter_error();

function raise_parameter_error()
    name=mfilename();
    error(['Illegal parameters, use one of:\n',...
        '- %s(...,''radius'',r) to use a radius of r voxels\n',...
        '- %s(...,''count'',c) to select c voxels per searchlight\n',...
        '(As of January 2014 the syntax of this function has changed)'],...
            name,name);


function t_obj = parcel_stats2statistic_image(atlas_obj, tscores, pvalues, dfe, sig)
% Utility that transforms parcel-wise t-statistics and p-values into a statistic_image object in voxelwise image space
% Expands parcel values so that they are replicated for each voxel in the parcel.
%
% :Usage:
% ::
%
% t_obj = parcel_stats2statistic_image(atlas_obj, tscores, pvalues, dfe, sig)
%
% For objects: Type methods(object_name) for a list of special commands
%              Type help object_name.method_name for help on specific
%              methods.
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2021 Tor Wager
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..
%
% :Inputs:
%
%   **atlas_obj:**
%        An atlas-class object with k distinct regions or parcels
%
%   **tscores:**
%        n x k matrix of k t-scores for each of n parcels, where k is,
%        e.g., the number of predictors tested in a multiple regression
%
%   **pvalues:**
%        n x k matrix of k p-values for each of n parcels
%
%   **dfe:**
%        n x 1 matrix of error degrees of freedom for each of n parcels
%
%   **sig:**
%        n x k matrix of k significant tests for each of n parcels
%        This may be, e.g., corrected for multiple comparisons.
%
% :Optional Inputs:
%
% :Outputs:
%
%   **t_obj:**
%        A statistic_image object containing t-maps. .dat contains
%        t-scores, .p contains p-values. Use statistic_image methods like
%        montage(t_obj), orthviews(t_obj), threshold(t_obj) to interact
%        with this object.
%
%
% :Examples:
% ::
%
% :References:
%   None
%
% :See also:
%   parcel_data2fmri_data
%

% Create placeholder statistic image

atlas_obj = replace_empty(atlas_obj);
k = size(tscores, 2);
placeholder_vec = ones(atlas_obj.volInfo.n_inmask, k);
all_parcel_idx = double(atlas_obj.dat);
u = unique(all_parcel_idx); u(u == 0) = [];

% initialize variables with correct size

t_obj = statistic_image('dat', single(0 .* placeholder_vec), ...
    'p', placeholder_vec, ...
    'sig', logical(placeholder_vec), ...
    'type', 'T', ...
    'dfe', placeholder_vec, ...
    'volInfo', atlas_obj.volInfo);

% number of parcels

n = size(tscores, 1); % num parcels

if n ~= length(u)
        error('Parcel indices in atlas_obj and extracted parcel-wise t-values do not match. Check code.')
end

t_obj.dat = zeros(size(placeholder_vec, 1), k); % voxels x conditions

    for j = 1:length(u)
        % For each parcel, fill in statistic_image object
        % ----------------------------------------------------
        parcelidx = u(j);

        wh_vox = all_parcel_idx == parcelidx;

        % map parcels to voxels
        t_obj.dat(wh_vox, :) = repmat(tscores(j, :), sum(wh_vox), 1);
        t_obj.p(wh_vox, :) = repmat(pvalues(j, :), sum(wh_vox), 1);
        t_obj.dfe(wh_vox, :) = repmat(dfe(j, 1), sum(wh_vox), k);       % replicate dfe
        t_obj.sig(wh_vox, :) = repmat(sig(j, :), sum(wh_vox), 1);

    end

t_obj = enforce_variable_types(t_obj);  % space-saving: 5/24/17

% input_struct.t_statistic_obj = t_obj;

end % function


function [innermost, inside] = find_innermost_boundary(bnd)

% FIND_INNERMOST_BOUNDARY locates innermost compartment of a BEM model
% by looking at the containment of the triangular meshes describing
% the surface boundaries
%
% [innermost] = find_innermost_boundary(bnd)
%
% with the boundaries described by a struct-array bnd with
%   bnd(i).pnt  vertices of boundary i (matrix of size Nx3)
%   bnd(i).tri  triangles of boundary i (matrix of size Mx3)

% Copyright (C) 2003, 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$

ncmp = length(bnd);

if ncmp==1
  innermost = 1;
  return
end

% try to locate the innermost compartment
for i=1:ncmp
for j=1:ncmp
  % determine for a single vertex on each surface if it is inside or outside the other surfaces
  curpos1 = bnd(i).pos(1,:); % any point on the boundary is ok
  curpos  = bnd(j).pos;
  curtri  = bnd(j).tri;
  if i==j
    inside(i,j) = 0;
  else
    inside(i,j) = bounding_mesh(curpos1, curpos, curtri);
  end
end
end
% assume that the sources are in the innermost compartment
tmp = sum(inside, 2);
[i, innermost] = max(tmp);


function result = swt16rec3_GPU(dec_full, level, waveletName)
%SWT16rec3 Stationary wavelet-based 2-level frame in 3-D.
%   swt16rec3_GPU performs a multi-level 3-D stationary wavelet synthesis
%   using a specific waveletName.
%
%   result = swt16rec3_GPU(dec_full, level, waveletName) computes the
%   3-D matrix 'result' at using the coefficients in 'dec_full', level
%   'level' and using 'waveletName'. Level must be either 1 or 2. Wavelet Toolbox
%   is required to retrieve the filters using 'waveletName'. Both 'level'
%   and 'waveletName' should match those used in creating 'dec_full'.
%
%   Requires the Parallel Computing Toolbox and a compatible GPU.
%
%   See conv3DGPU and swt16dec3_GPU

%   A. Kucukelbir 12-June-2012.
%   Last Revision: 26-June-2012.

% Create filters
if ischar(waveletName)
    [~,~,LR,HR] = wfilters(waveletName);
end

% Define kernels to be used by GPU
rowsKernel    = parallel.gpu.CUDAKernel('conv3DGPU.ptx', 'conv3DGPU.cu', 'rowsKernel');
columnsKernel = parallel.gpu.CUDAKernel('conv3DGPU.ptx', 'conv3DGPU.cu', 'columnsKernel');
beamsKernel   = parallel.gpu.CUDAKernel('conv3DGPU.ptx', 'conv3DGPU.cu', 'beamsKernel');
halfSum       = parallel.gpu.CUDAKernel('conv3DGPU.ptx', 'conv3DGPU.cu', 'halfSum');

% Upsample filters and create temporary cell
x = cell(1,1);
for i = 1:level-1
   LR = dyadup(LR,1);
   HR = dyadup(HR,1);
end

% Set parameters for convolution kernels on GPU
BLOCKDIM_X = 8;
BLOCKDIM_Y = 8;
BLOCKDIM_Z = 8;

for i = 1:level

    % Grab first level of frame coeffieicents.
    dec = dec_full(1:8);
    dec_full(1:8) = [];

    % Downsample filters
    if(i==2)
        LR = dyaddown(LR,0);
        HR = dyaddown(HR,0);
    end

    % Move filters to GPU
    LR_g = gpuArray(LR);
    HR_g = gpuArray(HR);

    % Calculate kernel radius
    if( mod(length(LR),2) == 0 )
        kernelRadius = length(LR)/2;
    else
        kernelRadius = (length(LR)-1)/2;
    end

    % Ensure volume has dimensions that are a multiple of 8
    diff = 0;
    if( mod(size(dec{1,1},1),8) ~= 0 )
    diff = 8*ceil(size(dec{1,1},1)/8) - size(dec{1,1},1);
        for j=1:8
        dec{j,1} = padarray(dec{j,1},[diff diff diff],'post');
        end
    end

    % Calculate volume sizes and transfer to GPU
    volRowSize = size(dec{1,1},1);
    volColSize = size(dec{1,1},2);
    volBeaSize = size(dec{1,1},3);
    dec1_g = gpuArray(dec{1,1});
    dec2_g = gpuArray(dec{2,1});
    dec3_g = gpuArray(dec{3,1});
    dec4_g = gpuArray(dec{4,1});
    dec5_g = gpuArray(dec{5,1});
    dec6_g = gpuArray(dec{6,1});
    dec7_g = gpuArray(dec{7,1});
    dec8_g = gpuArray(dec{8,1});

    % Setup GPU grids and blocks
    rowsKernel.GridSize        = [(volRowSize / BLOCKDIM_X) * (volBeaSize / BLOCKDIM_Z) , (volColSize / BLOCKDIM_Y)];
    rowsKernel.ThreadBlockSize = [BLOCKDIM_X, BLOCKDIM_Y, BLOCKDIM_Z];

    columnsKernel.GridSize        = [(volRowSize / BLOCKDIM_X) * (volBeaSize / BLOCKDIM_Z) , (volColSize / BLOCKDIM_Y)];
    columnsKernel.ThreadBlockSize = [BLOCKDIM_X, BLOCKDIM_Y, BLOCKDIM_Z];

    beamsKernel.GridSize        = [(volRowSize / BLOCKDIM_X) * (volBeaSize / BLOCKDIM_Z) , (volColSize / BLOCKDIM_Y)];
    beamsKernel.ThreadBlockSize = [BLOCKDIM_X, BLOCKDIM_Y, BLOCKDIM_Z];

    halfSum.GridSize        = [(volRowSize / BLOCKDIM_X) * (volBeaSize / BLOCKDIM_Z) , (volColSize / BLOCKDIM_Y)];
    halfSum.ThreadBlockSize = [BLOCKDIM_X, BLOCKDIM_Y, BLOCKDIM_Z];

    % Do the SWT for this level
    tmp_g = swt3_1level(dec1_g, dec2_g, dec3_g, dec4_g, dec5_g, dec6_g, dec7_g, dec8_g,...
                      LR_g, HR_g, ...
                      volRowSize, volColSize, volBeaSize, kernelRadius,...
                      rowsKernel, columnsKernel, beamsKernel, halfSum);

    % Remove multiple of 8 padding
    tmp = gather(tmp_g);
    x{i,1} = tmp(1:end-diff, 1:end-diff, 1:end-diff);

    % Remove kernelRadius padding
    if( mod(length(LR),2) == 0 )
            x{i,1} = x{i,1}(kernelRadius:end-(kernelRadius-1)-1, kernelRadius:end-(kernelRadius-1)-1, kernelRadius:end-(kernelRadius-1)-1);
    else
            x{i,1} = x{i,1}(kernelRadius+1:end-kernelRadius, kernelRadius+1:end-kernelRadius, kernelRadius+1:end-kernelRadius);
    end

    % Take care of second level specific processing
    if (i == 2)
        x{1,1} = padarray(x{1,1},[diff diff diff],'post');
        dec1_g = gpuArray(x{1,1});
            tmp_g = swt3_1level(dec1_g, dec2_g, dec3_g, dec4_g, dec5_g, dec6_g, dec7_g, dec8_g,...
                      LR_g, HR_g, ...
                      volRowSize, volColSize, volBeaSize, kernelRadius,...
                      rowsKernel, columnsKernel, beamsKernel, halfSum);
        tmp = gather(tmp_g);
        tmp = tmp(1:end-diff, 1:end-diff, 1:end-diff);
        if( mod(length(LR),2) == 0 )
            x{1,1} = tmp(kernelRadius:end-(kernelRadius-1)-1, kernelRadius:end-(kernelRadius-1)-1, kernelRadius:end-(kernelRadius-1)-1);
        else
            x{1,1} = tmp(kernelRadius:end-kernelRadius-1, kernelRadius:end-kernelRadius-1, kernelRadius:end-kernelRadius-1);
        end
    end

end

% Combine results from all levels
result = zeros(size(x{1,1}));
for i = 1:level
    result = result + 1/level*x{i,1};
end

end

function x = swt3_1level(dec1_g, dec2_g, dec3_g, dec4_g, dec5_g, dec6_g, dec7_g, dec8_g,...
                  LR_g, HR_g, ...
                  volRowSize, volColSize, volBeaSize, kernelRadius,...
                  rowsKernel, columnsKernel, beamsKernel, halfSum)

%% FIRST PASS ALONG BEAMS
% Get the AA
aaa_Lo_Lo_Lo_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
daa_Lo_Lo_Hi_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
aa_Lo_Lo_g     = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);

aaa_Lo_Lo_Lo_g = feval( beamsKernel, aaa_Lo_Lo_Lo_g, dec1_g, LR_g, volRowSize, volColSize, volBeaSize, kernelRadius );
daa_Lo_Lo_Hi_g = feval( beamsKernel, daa_Lo_Lo_Hi_g, dec2_g, HR_g, volRowSize, volColSize, volBeaSize, kernelRadius );

aa_Lo_Lo_g = feval( halfSum, aa_Lo_Lo_g, aaa_Lo_Lo_Lo_g, daa_Lo_Lo_Hi_g, volRowSize, volColSize, volBeaSize );

% Get the DA
ada_Lo_Hi_Lo_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
dda_Lo_Hi_Hi_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
da_Lo_Hi_g     = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);

ada_Lo_Hi_Lo_g = feval( beamsKernel, ada_Lo_Hi_Lo_g, dec3_g, LR_g, volRowSize, volColSize, volBeaSize, kernelRadius );
dda_Lo_Hi_Hi_g = feval( beamsKernel, dda_Lo_Hi_Hi_g, dec4_g, HR_g, volRowSize, volColSize, volBeaSize, kernelRadius );

da_Lo_Hi_g = feval( halfSum, da_Lo_Hi_g, ada_Lo_Hi_Lo_g, dda_Lo_Hi_Hi_g, volRowSize, volColSize, volBeaSize );

% Get the AD
aad_Hi_Lo_Lo_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
dad_Hi_Lo_Hi_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
ad_Hi_Lo_g     = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);

aad_Hi_Lo_Lo_g = feval( beamsKernel, aad_Hi_Lo_Lo_g, dec5_g, LR_g, volRowSize, volColSize, volBeaSize, kernelRadius );
dad_Hi_Lo_Hi_g = feval( beamsKernel, dad_Hi_Lo_Hi_g, dec6_g, HR_g, volRowSize, volColSize, volBeaSize, kernelRadius );

ad_Hi_Lo_g = feval( halfSum, ad_Hi_Lo_g, aad_Hi_Lo_Lo_g, dad_Hi_Lo_Hi_g, volRowSize, volColSize, volBeaSize );

% Get the DD
add_Hi_Hi_Lo_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
ddd_Hi_Hi_Hi_g = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
dd_Hi_Hi_g     = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);

add_Hi_Hi_Lo_g = feval( beamsKernel, add_Hi_Hi_Lo_g, dec7_g, LR_g, volRowSize, volColSize, volBeaSize, kernelRadius );
ddd_Hi_Hi_Hi_g = feval( beamsKernel, ddd_Hi_Hi_Hi_g, dec8_g, HR_g, volRowSize, volColSize, volBeaSize, kernelRadius );

dd_Hi_Hi_g = feval( halfSum, dd_Hi_Hi_g, add_Hi_Hi_Lo_g, ddd_Hi_Hi_Hi_g, volRowSize, volColSize, volBeaSize );

%% SECOND PASS ALONG COLUMNS
% Get the A
aa_Lo_Lo_gt = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
da_Lo_Hi_gt = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
a_Lo_g      = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);

aa_Lo_Lo_gt = feval( columnsKernel, aa_Lo_Lo_gt, aa_Lo_Lo_g, LR_g, volRowSize, volColSize, volBeaSize, kernelRadius );
da_Lo_Hi_gt = feval( columnsKernel, da_Lo_Hi_gt, da_Lo_Hi_g, HR_g, volRowSize, volColSize, volBeaSize, kernelRadius );

a_Lo_g = feval( halfSum, a_Lo_g, aa_Lo_Lo_gt, da_Lo_Hi_gt, volRowSize, volColSize, volBeaSize );

% Get the D
ad_Hi_Lo_gt = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
dd_Hi_Hi_gt = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
d_Hi_g      = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);

ad_Hi_Lo_gt = feval( columnsKernel, ad_Hi_Lo_gt, ad_Hi_Lo_g, LR_g, volRowSize, volColSize, volBeaSize, kernelRadius );
dd_Hi_Hi_gt = feval( columnsKernel, dd_Hi_Hi_gt, dd_Hi_Hi_g, HR_g, volRowSize, volColSize, volBeaSize, kernelRadius );

d_Hi_g = feval( halfSum, d_Hi_g, ad_Hi_Lo_gt, dd_Hi_Hi_gt, volRowSize, volColSize, volBeaSize );

%% FIRST PASS ALONG ROWS
a_Lo_gt = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
d_Hi_gt = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);
x       = parallel.gpu.GPUArray.zeros(volRowSize, volColSize, volBeaSize);

a_Lo_gt = feval( rowsKernel, a_Lo_gt, a_Lo_g, LR_g, volRowSize, volColSize, volBeaSize, kernelRadius );
d_Hi_gt = feval( rowsKernel, d_Hi_gt, d_Hi_g, HR_g, volRowSize, volColSize, volBeaSize, kernelRadius );

x = feval( halfSum, x, a_Lo_gt, d_Hi_gt, volRowSize, volColSize, volBeaSize );

end


function Ne=vertex_neighbours_double(Fa,Fb,Fc,Vx,Vy,Vz)

F=[Fa Fb Fc];
V=[Vx Vy Vz];

% Neighbourh cell array
Ne=cell(1,size(V,1));

% Loop through all faces
for i=1:length(F)
    % Add the neighbors of each vertice of a face
    % to his neighbors list.
    Ne{F(i,1)}=[Ne{F(i,1)} [F(i,2) F(i,3)]];
    Ne{F(i,2)}=[Ne{F(i,2)} [F(i,3) F(i,1)]];
    Ne{F(i,3)}=[Ne{F(i,3)} [F(i,1) F(i,2)]];
end

% Loop through all neighbor arrays and sort them (Rotation same as faces)
for i=1:size(V,1)

    Pneighf=Ne{i};
    if(isempty(Pneighf))
        Pneig=[];
    else
        start=1;
        for index1=1:2:length(Pneighf)
            found=false;
            for index2=2:2:length(Pneighf),
                if(Pneighf(index1)==Pneighf(index2))
                    found=true; break
                end
            end
            if(~found)
                start=index1; break
            end
        end
        Pneig=[];
        Pneig(1)=Pneighf(start);
        Pneig(2)=Pneighf(start+1);

        % Add the neighbours with respect to original rotation
        for j=2+double(found):(length(Pneighf)/2)
            found = false;
            for index=1:2:length(Pneighf),
                if(Pneighf(index)==Pneig(end))
                    if(sum(Pneig==Pneighf(index+1))==0)
                        found =true;
                        Pneig=[Pneig Pneighf(index+1)];
                    end
                end
            end
            if(~found) % This only happens with weird edge vertices
                for index=1:2:length(Pneighf),
                    if(sum(Pneig==Pneighf(index))==0)
                        Pneig=[Pneig Pneighf(index)];
                        if(sum(Pneig==Pneighf(index+1))==0)
                            Pneig=[Pneig Pneighf(index+1)];
                        end
                    end
                end
            end
        end
        % Add forgotten neigbours
        if(length(Pneig)<length(Pneighf))
            for j=1:length(Pneighf)
                if(sum(Pneig==Pneighf(j))==0)
                    Pneig=[Pneig Pneighf(j)];
                end
            end
        end
    end
    Ne{i}=Pneig;
end


function A00 = synA00Qmin(A11, sizeA00, cmax)
%-----------------------------------------------------------------------------
%
% For each point of colour 00 this function assigns the minimum value at the
% neighbouring gridpoints of colour 11.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: December 12, 2001.
% (c) 1998-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
[n11, m11]=size(A11);
if nargin == 3
  o=[0 0];
  if ~all(size(o) == size(sizeA00))
    error(' synA00Qmin - unexpected dimensions of sizeA00 ')
  else
    clear o;
    n00=sizeA00(1);
    m00=sizeA00(2);
  end
elseif nargin == 2
  n00=n11+1;
  m00=m11+1;
else
  error(' synA00Qmin - number of arguments should be either 2 or 3 ')
end
%[n00, m00]=size(A00);
if m00 == m11+1
  if n00 == n11+1
    S = min(extL(A11, cmax), extR(A11, cmax));
    A00=min(extD(S, cmax), extU(S, cmax));
  elseif n00 == n11
    S = min(extL(A11, cmax), extR(A11, cmax));
    A00=min(S, stripD(extU(S, cmax)));
  else
    disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str([n00 m00])]);
    error(' synA00Qmin - A11 and target A00 do not match ');
  end
elseif m00 == m11
  if n00 == n11+1
    S = min(stripR(extL(A11, cmax)), A11);
    A00=min(extD(S, cmax), extU(S, cmax));
  elseif n00 == n11
    S = min(stripR(extL(A11, cmax)), A11);
    A00=min(S, stripD(extU(S, cmax)));
  else
    disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str([n00 m00])]);
    error(' synA00Qmin - A11 and target A00 do not match ');
  end
else
  disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str([n00 m00])]);
  error(' synA00Qmin - A11 and target A00 do not match ');
end
%------------------------------------------------------------------------------


classdef AMICO_ACTIVEAX

properties
    id, name                % id and name of the model
    dPar                    % parallel diffusivity of the tensors [units of mm^2/s]
    dIso                    % isotropic diffusivity [units of mm^2/s]
    IC_Rs                   % radii of the axons [units of 1E-6 (micrometers)]
    IC_VFs                  % volume fractions of the axons
    OUTPUT_names            % suffix of the output maps
    OUTPUT_descriptions     % description of the output maps
end

methods

    % =================================
    % Setup the parameters of the model
    % =================================
    function obj = AMICO_ACTIVEAX()
        global CONFIG

        % set the parameters of the model
        obj.id        = 'ACTIVEAX';
        obj.name      = 'ActiveAx';
        obj.dPar      = 0.6 * 1E-3;
        obj.dIso      = 2.0 * 1E-3;
        obj.IC_Rs     = [0.01 linspace(0.5,10,20)];
        obj.IC_VFs    = [0.3:0.1:0.9];

        obj.OUTPUT_names        = { 'v', 'a', 'd' };
        obj.OUTPUT_descriptions = {'Intra-cellular volume fraction', 'Mean axonal diameter', 'Axonal density'};

        % set the parameters to fit it
        CONFIG.OPTIMIZATION.SPAMS_param.mode    = 2;
        CONFIG.OPTIMIZATION.SPAMS_param.pos     = true;
        CONFIG.OPTIMIZATION.SPAMS_param.lambda  = 0.25; % l1 regularization
        CONFIG.OPTIMIZATION.SPAMS_param.lambda2 = 4;    % l2 regularization
    end

    % ==================================================================
    % Generate high-resolution kernels and rotate them in harmonic space
    % ==================================================================
    function GenerateKernels( obj, ATOMS_path, schemeHR, AUX, idx_IN, idx_OUT )
        global CONFIG AMICO_data_path CAMINO_path

        % check if high-resolution scheme has been created
        schemeHrFilename = fullfile(ATOMS_path,'protocol_HR.scheme');
        if ~exist( schemeHrFilename, 'file' )
            error( '[AMICO_GenerateKernels_ACTIVEAX] File "protocol_HR.scheme" not found in folder "%s"', ATOMS_path )
        end

        filenameHr = [tempname '.Bfloat'];
        progress = ProgressBar( numel(obj.IC_Rs) + numel(obj.IC_VFs) + 1 );

        % Restricted
        % ==========
        for R = obj.IC_Rs
            % generate
            if exist( filenameHr, 'file' ), delete( filenameHr ); end
            CMD = sprintf( '%s/datasynth -synthmodel compartment 1 CYLINDERGPD %E 0 0 %E -schemefile %s -voxels 1 -outputfile %s 2> /dev/null', CAMINO_path, obj.dPar*1e-6, R*1e-6, schemeHrFilename, filenameHr );
            [status result] = system( CMD );
            if status>0
                disp(result)
                error( '[AMICO_ACTIVEAX.GenerateKernels] Problems generating the signal with datasynth' );
            end

            % rotate and save
            fid = fopen( filenameHr, 'r', 'b' );
            signal = fread(fid,'float');
            fclose(fid);
            delete( filenameHr );
            lm = AMICO_RotateKernel( signal, AUX, idx_IN, idx_OUT, false );
            save( fullfile( ATOMS_path, sprintf('A_%03d.mat',progress.i) ), '-v6', 'lm' )
            progress.update();
        end

        % Hindered
        % ========
        for ICVF = obj.IC_VFs
            % generate
            d_perp = obj.dPar * ( 1.0 - ICVF );
            if exist( filenameHr, 'file' ), delete( filenameHr ); end
            CMD = sprintf( '%s/datasynth -synthmodel compartment 1 ZEPPELIN %E 0 0 %E -schemefile %s -voxels 1 -outputfile %s 2> /dev/null', CAMINO_path, obj.dPar*1e-6, d_perp*1e-6, schemeHrFilename, filenameHr );
            [status result] = system( CMD );
            if status>0
                disp(result)
                error( '[AMICO_ACTIVEAX.GenerateKernels] problems generating the signal' );
            end

            % rotate and save
            fid = fopen( filenameHr, 'r', 'b' );
            signal = fread(fid,'float');
            fclose(fid);
            delete( filenameHr );
            lm = AMICO_RotateKernel( signal, AUX, idx_IN, idx_OUT, false );
            save( fullfile( ATOMS_path, sprintf('A_%03d.mat',progress.i) ), '-v6', 'lm' )

            progress.update();
        end

        % Isotropic
        % =========
        % generate
        if exist( filenameHr, 'file' ), delete( filenameHr ); end
        CMD = sprintf( '%s/datasynth -synthmodel compartment 1 BALL %E -schemefile %s -voxels 1 -outputfile %s 2> /dev/null', CAMINO_path, obj.dIso*1e-6, schemeHrFilename, filenameHr );
        [status result] = system( CMD );
        if status>0
            disp(result)
            error( '[AMICO_ACTIVEAX.GenerateKernels] problems generating the signal' );
        end

        % resample and save
        fid = fopen( filenameHr, 'r', 'b' );
        signal = fread(fid,'float');
        fclose(fid);
        delete( filenameHr );
        lm = AMICO_RotateKernel( signal, AUX, idx_IN, idx_OUT, true );
        save( fullfile( ATOMS_path, sprintf('A_%03d.mat',progress.i) ), '-v6', 'lm' )
        progress.update();

        progress.close();
    end

    % ==============================================
    % Project kernels from harmonic to subject space
    % ==============================================
    function ResampleKernels( obj, ATOMS_path, idx_OUT, Ylm_OUT )
        global CONFIG AMICO_data_path KERNELS

        % Setup the KERNELS structure
        % ===========================
        nIC = numel(obj.IC_Rs);
        nEC = numel(obj.IC_VFs);

        KERNELS = {};
        KERNELS.model    = 'ACTIVEAX';
        KERNELS.nS       = CONFIG.scheme.nS;
        KERNELS.nA       = nIC + nEC + 1; % number of atoms

        KERNELS.dPar     = obj.dPar;

        KERNELS.Aic      = zeros( [KERNELS.nS nIC 181 181], 'single' );
        KERNELS.Aic_R    = zeros( 1, nIC, 'single' );

        KERNELS.Aec      = zeros( [KERNELS.nS nEC 181 181], 'single' );
        KERNELS.Aec_icvf = zeros( 1, nEC, 'single' );

        KERNELS.Aiso     = zeros( [KERNELS.nS 1], 'single' );
        KERNELS.Aiso_d   = NaN;

        progress = ProgressBar( KERNELS.nA );

        % Restricted
        % ==========
        for i = 1:nIC
            load( fullfile( ATOMS_path, sprintf('A_%03d.mat',progress.i) ), 'lm' );
            KERNELS.Aic(:,i,:,:) = AMICO_ResampleKernel( lm, idx_OUT, Ylm_OUT, false );
            KERNELS.Aic_R(i)     = obj.IC_Rs(i);
            progress.update();
        end

        % Hindered
        % ========
        for i = 1:nEC
            load( fullfile( ATOMS_path, sprintf('A_%03d.mat',progress.i) ), 'lm' );
            KERNELS.Aec(:,i,:,:) = AMICO_ResampleKernel( lm, idx_OUT, Ylm_OUT, false );
            KERNELS.Aec_icvf(i)  = obj.IC_VFs(i);
            progress.update();
        end

        % Isotropic
        % =========
        load( fullfile( ATOMS_path, sprintf('A_%03d.mat',progress.i) ), 'lm' );
        KERNELS.Aiso   = AMICO_ResampleKernel( lm, idx_OUT, Ylm_OUT, true );
        KERNELS.Aiso_d = obj.dIso;
        progress.update();

        progress.close();
    end

    % ===========================
    % Fit the model to each voxel
    % ===========================
    function [ MAPs ] = Fit( obj, y, i1, i2 )
        global CONFIG KERNELS

        % prepare SIGNAL and DICTIONARY
        if numel(KERNELS.Aiso_d) > 0
            A = double( [ KERNELS.Aic(CONFIG.scheme.dwi_idx,:,i1,i2) KERNELS.Aec(CONFIG.scheme.dwi_idx,:,i1,i2) KERNELS.Aiso(CONFIG.scheme.dwi_idx) ] );
        else
            A = double( [ KERNELS.Aic(CONFIG.scheme.dwi_idx,:,i1,i2) KERNELS.Aec(CONFIG.scheme.dwi_idx,:,i1,i2) ] );
        end
        AA = [ ones(1,KERNELS.nA) ; A ];
        yy = [ 1 ; y(CONFIG.scheme.dwi_idx) ];

        % estimate coefficients
        x = full( mexLasso( yy, AA, CONFIG.OPTIMIZATION.SPAMS_param ) );

        % compute MAPS
        nIC = numel(obj.IC_Rs);
        nEC = numel(obj.IC_VFs);
        f1 = sum( x( 1:nIC ) );
        f2 = sum( x( (nIC+1):(nIC+nEC) ) );
        MAPs(1) = f1 / ( f1 + f2 + eps );                           % intra-cellular volume fraction (v)
        MAPs(2) = 2 * KERNELS.Aic_R * x(1:nIC) / ( f1 + eps );      % mean axonal diameter
        MAPs(3) = (4*MAPs(1)) / ( pi*MAPs(2)^2 + eps );             % axonal density
    end

end

end


function model = assignQuantDir(model)
% Quantitatively assigns reaction directionality based on estimated bounds
% on transformed reaction Gibbs energies
%
% USAGE:
%
%    model = assignQuantDir(model)
%
% INPUT:
%    model:    structure with fields:
%
%                * .SIntRxnBool - `n x 1` boolean of internal reactions
%                * .DrGtMin - `n x 1` array of estimated lower bounds on
%                  transformed reaction Gibbs energies.
%                * .DrGtMax - `n x 1` array of estimated upper bounds on
%                  transformed reaction Gibbs energies.
%
% OUTPUT:
%    model:    structure with fields:
%
%                * .quantDir - `n x 1` array indicating quantitatively assigned
%                  reaction directionality. 1 for reactions that are
%                  irreversible in the forward direction, -1 for
%                  reactions that are irreversible in the reverse
%                  direction, and 0 for reversible reactions.
%
% .. Author: - Hulda SH, Nov. 2012

model.NaNdfG0MetBool=isnan(model.DfGt0);
model.NaNd0GRxnBool = isnan(model.DrGt0);

DrGtMin=model.DrGtMin;
DrGtMax=model.DrGtMax;

[nMet,nRxn]=size(model.S);

% model.quantDir = zeros(size(DrGtMin));
% model.quantDir(DrGtMax < 0) = 1;
% model.quantDir(DrGtMin > 0) = -1;

nEqualDrGt=nnz(DrGtMin==DrGtMax & DrGtMin~=0);
if any(nEqualDrGt)
    fprintf('%s\n',[num2str(nEqualDrGt) '/' num2str(length(DrGtMin)) ' reactions with DrGtMin=DrGtMax~=0' ]);
end

nZeroDrGt=nnz(DrGtMin==0 & DrGtMax==0);
if any(nZeroDrGt)
    fprintf('%s\n',[num2str(nZeroDrGt) '/' num2str(length(DrGtMin)) ' reactions with DrGtMin=DrGtMax=0' ]);
end

if any(model.NaNd0GRxnBool)
    warning('Some DfGt0 are NaN');
end

if any(DrGtMin>DrGtMax)
    error('DrGtMin greater than DrGtMax');
end

%reaction directionality
%keep exchange bounds the same as for the recostruction
model.lb_reconThermo=model.lb;
model.ub_reconThermo=model.ub;

%now set internal reaction directions
for n=1:nRxn
    if model.SIntRxnBool(n)
        if model.NaNd0GRxnBool(n)
            %for the reactions that involve a NaN metabolite standard Gibbs energy of
            %formation, use the directions given by the reconstruction
            if model.lb(n)<0 && model.ub(n)>0
                model.lb_reconThermo(n)=-Inf;
                model.ub_reconThermo(n)=Inf;
            end
            %forward
            if model.lb(n)>=0 && model.ub(n)>0
                model.lb_reconThermo(n)=0;
                model.ub_reconThermo(n)=Inf;
            end
            %reverse
            if model.lb(n)<0 && model.ub(n)<=0
                model.lb_reconThermo(n)=-Inf;
                model.ub_reconThermo(n)=0;
            end
            if model.lb(n)==0 && model.ub(n)==0
                error(['Reaction ' model.rxns{n} ' bounds set to zero'])
            end
            %note that there is no thermodynamic directionality assignment
            %for this reaction
            model.directionalityThermo{n}=NaN;
        else
            if DrGtMax(n)<0
                model.directionalityThermo{n}='forward';
                model.lb_reconThermo(n)=0;
                model.ub_reconThermo(n)=Inf;
            end
            if DrGtMin(n)>0
                model.directionalityThermo{n}='reverse';
                model.lb_reconThermo(n)=-Inf;
                model.ub_reconThermo(n)=0;
            end
            if DrGtMin(n)<0 && DrGtMax(n)>0
                model.directionalityThermo{n}='reversible';
                model.lb_reconThermo(n)=-Inf;
                model.ub_reconThermo(n)=Inf;
            end
            if DrGtMin(n)==DrGtMax(n)
                model.directionalityThermo{n}='equilibrium';
            end
            if model.lb(n)==0 && model.ub(n)==0
                error(['Reaction ' model.rxns{n} ' bounds set to zero'])
            end
        end
    end
end


function G = bst_meg_sph(L, Channel, Param)
% BST_MEG_SPH: Calculate the (overlapping) sphere models for MEG
%
% USAGE:  G = bst_meg_sph(L, Channel, Param);
%
% INPUT:
%    - L        : a 3 x nL array, each column a source location (x y z coordinates); nL sources
%    - Channel  : a Brainstorm channel structure
%    - Param[]  : array of structures (one per channel)
%        |- Center  : a vector of the x, y, z locations for the sphere model
%        |            (assume the same center for every sphere for the classical spherical head model)
%
% OUTPUT:
%    - G  : the gain matrix: each column is the forward field of each source

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@

%% ===== PARSE INPUTS =====
% Sources locations should be 3 x m
if(size(L,1)~=3)
    error('Matrix not given as 3 x n. Correct calling code');
end
% Check that the number of coils is the same for all the channels
chanCoils = cellfun(@(c)size(c,2), {Channel.Loc});
grpCoils = unique(chanCoils);
% If there are multiple sensor sensor types (different numbres of coils)
if (length(grpCoils) > 1)
    % This function can only accept calls to groups of sensors with the same number of coils
    % => Group the sensors by number of coils and call os_meg as many times as needed
    G = NaN * zeros(length(Channel), 3 * size(L,2));
    for iGrp = 1:length(grpCoils)
        % Get all the sensors with this amount of coils
        iMegGrp = find(chanCoils == grpCoils(iGrp));
        % Compute (os_meg)
        G(iMegGrp,:) = bst_meg_sph(L, Channel(iMegGrp), Param(iMegGrp));
    end
    return;
end
% Number of coils for this call
NumCoils = chanCoils(1);

%% ===== COMPUTATION =====
% Get locations
AllLocs = [Channel.Loc];
AllLocs = reshape(AllLocs, NumCoils*3, size(AllLocs,2)/NumCoils);
% Get orientations
AllOrient = [Channel.Orient];
AllOrient = AllOrient * bst_inorcol(AllOrient);
AllOrient = reshape(AllOrient, NumCoils*3, size(AllOrient,2)/NumCoils);
% Get weights
AllWeight = [Channel.Weight];
AllWeight = reshape(AllWeight(:), NumCoils, length(AllWeight(:))/NumCoils);

% Process each group of coils
G = 0;
for j = 1:NumCoils
    % P.sensor is 3 x nR,each column a sensor location
    % P.orient is 3 x nR, the sensor orientation
    % P.center is 3 x nR, the sphere center for each sensor
    P.sensor = AllLocs((-2:0) + j*3, :);
    P.orient = AllOrient((-2:0) + j*3, :);
    P.weight = AllWeight(j,:);
    P.center = [Param.Center];
    % Local call below
    G = G + sarvas(L, P);
end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%  Local Sarvas functions %%%%%%%%%%%%%%%%%%%%%%%%
function G = sarvas(L, P)
% Bronzan Sarvas forward model, spherical head
% P.sensor is 3 x nR,each column a sensor location
% P.orient is 3 x nR, the sensor orientation
% P.center is 3 x nR, the sphere center for each sensor

if(~isfield(P,'center')), % user did not provide
   P.center = []; % initialize to null
end
if(isempty(P.center)), % user gave as null
   P.center = zeros(size(P.sensor));  % set to coordinate origin
end

P.sensor = P.sensor - P.center; % shift sensor coordinates

iMag = find(sum(P.sensor.^2,1) == 0); % Indices of channels located at P.center.
if ~isempty(iMag)
    P.sensor(:,iMag) = repmat([1 1 1]',1,length(iMag)); % Move them away (arbitrary location).
end

nR = size(P.sensor,2); % number of sensors
nL = size(L,2);  % number of source points

Rn2 = sum(P.sensor.^2,1); % distance to sensor squared
Rn = sqrt(Rn2); % distance

if (nR >= nL), % more sensors than dipoles
   G = zeros(nR,3*nL);  % gain matrix
   for Li = 1:nL,
      Lmat = L(:,Li+zeros(1,nR)); % matrix of location repeated
      Lmat = Lmat - P.center; % each center shifted relative to its center
      D = P.sensor - Lmat;  % distance from souce to sensors
      Dn2 = sum(D.^2,1); % distance squared
      Dn = sqrt(Dn2);  % distance
      R_dot_D = sum(P.sensor .* D);  % dot product of sensor and distance
      R_dot_Dhat = R_dot_D ./ Dn;  % dot product of sensor and distance

      F = Dn2 .* Rn + Dn .* R_dot_D;  % Sarvas' function F

      GF_dot_o = Dn2 .* sum(P.sensor.*P.orient) ./ Rn + ...
         (2 * Rn + R_dot_Dhat) .* sum(D.*P.orient) + ...
         Dn .* sum((D+P.sensor).*P.orient);

      tempF = GF_dot_o ./ F.^2;

      temp = bst_cross(Lmat,P.orient) ./ F([1 1 1],:) - ...
             bst_cross(Lmat,P.sensor) .* tempF([1 1 1],:);
      G(:,Li*3+[-2 -1 0]) = temp';

   end

else  % more dipoles than sensors nL > nR
   G = zeros(3*nL,nR);  % gain matrix transposed

   for Ri = 1:nR,
      Rmat = P.sensor(:,Ri+zeros(1,nL)); % matrix of sensor repeated
      Omat = P.orient(:,Ri+zeros(1,nL)); % orientations
      Lmat = L - P.center(:,Ri+zeros(1,nL)); % shift centers to this coordinate

      D = Rmat - Lmat;
      Dn2 = sum(D.^2,1); % distance squared
      Dn = sqrt(Dn2);  % distance
      R_dot_D = sum(Rmat .* D);  % dot product of sensor and distance
      R_dot_Dhat = R_dot_D ./ Dn;  % dot product of sensor and distance

      F = Dn2 * Rn(Ri) + Dn .* R_dot_D;  % Sarvas' function F

      GF_dot_o = Dn2 * sum(P.sensor(:,Ri).*P.orient(:,Ri)) / Rn(Ri) + ...
         (2 * Rn(Ri) + R_dot_D ./ Dn) .* sum(D.*Omat) + ...
         Dn .* sum((D+Rmat).*Omat);

      tempF = GF_dot_o ./ F.^2;

      temp = bst_cross(Lmat,Omat) ./ F([1 1 1],:) - ...
             bst_cross(Lmat,Rmat) .* tempF([1 1 1],:);

      G(:,Ri) = temp(:);
   end

   G = G';
end

if(isfield(P,'weight')),
   Weights = P.weight(:); %make sure column
   % scale each row by its appropriate weight
   G = Weights(:,ones(1,size(G,2))) .* G;
end

G = G * 1e-7; % mu_o over 4 pi


function [transform, coordsys] = ft_headcoordinates(fid1, fid2, fid3, fid4, coordsys)

% FT_HEADCOORDINATES returns the homogeneous coordinate transformation matrix
% that converts the specified fiducials in any coordinate system (e.g. MRI)
% into the rotated and translated headcoordinate system.
%
% Use as
%   [transform, coordsys] = ft_headcoordinates(fid1, fid2, fid3, coordsys)
% or
%   [transform, coordsys] = ft_headcoordinates(fid1, fid2, fid3, fid4, coordsys)
%
% Depending on the desired coordinate system, the order of the fiducials is
% interpreted as follows
%
%   fid1 = nas
%   fid2 = lpa
%   fid3 = rpa
%   fid4 = extra point (optional)
%
%   fid1 = ac
%   fid2 = pc
%   fid3 = midsagittal
%   fid4 = extra point (optional)
%
%   fid1 = pt1
%   fid2 = pt2
%   fid3 = pt3
%   fid4 = extra point (optional)
%
%   fid1 = bregma
%   fid2 = lambda
%   fid3 = midsagittal
%   fid4 = extra point (optional)
%
% The fourth argument fid4 is optional and can be specified as an an extra point
% which is assumed to have a positive Z-coordinate. It will be used to ensure correct
% orientation of the Z-axis (ctf, 4d, bti, eeglab, yokogawa, neuromag, itab) or
% X-axis (acpc, spm, mni, tal). The specification of this extra point may result in
% the handedness of the transformation to be changed, but ensures consistency with
% the handedness of the input coordinate system.
%
% The coordsys input argument is a string that determines how the location of the
% origin and the direction of the axis is to be defined relative to the fiducials:
%   according to CTF conventions:             coordsys = 'ctf'
%   according to 4D conventions:              coordsys = '4d' or 'bti'
%   according to EEGLAB conventions:          coordsys = 'eeglab'
%   according to NEUROMAG conventions:        coordsys = 'itab'
%   according to ITAB conventions:            coordsys = 'neuromag'
%   according to YOKOGAWA conventions:        coordsys = 'yokogawa'
%   according to ASA conventions:             coordsys = 'asa'
%   according to FTG conventions:             coordsys = 'ftg'
%   according to ACPC conventions:            coordsys = 'acpc'
%   according to SPM conventions:             coordsys = 'spm'
%   according to MNI conventions:             coordsys = 'mni'
%   according to Talairach conventions:       coordsys = 'tal'
%   according to PAXINOS conventions:         coordsys = 'paxinos'
% If the coordsys input argument is not specified, it will default to 'ctf'.
%
% The CTF, 4D, YOKOGAWA and EEGLAB coordinate systems are defined as follows:
%   the origin is exactly between lpa and rpa
%   the X-axis goes towards nas
%   the Y-axis goes approximately towards lpa, orthogonal to X and in the plane spanned by the fiducials
%   the Z-axis goes approximately towards the vertex, orthogonal to X and Y
%
% The TALAIRACH, SPM and ACPC coordinate systems are defined as:
%   the origin corresponds with the anterior commissure
%   the Y-axis is along the line from the posterior commissure to the anterior commissure
%   the Z-axis is towards the vertex, in between the hemispheres
%   the X-axis is orthogonal to the midsagittal-plane, positive to the right
%
% The NEUROMAG and ITAB coordinate systems are defined as follows:
%   the X-axis is from the origin towards the RPA point (exactly through)
%   the Y-axis is from the origin towards the nasion (exactly through)
%   the Z-axis is from the origin upwards orthogonal to the XY-plane
%   the origin is the intersection of the line through LPA and RPA and a line orthogonal to L passing through the nasion
%
% The ASA coordinate system is defined as follows:
%   the origin is at the orthogonal intersection of the line from rpa-lpa and the line trough nas
%   the X-axis goes towards nas
%   the Y-axis goes through rpa and lpa
%   the Z-axis goes approximately towards the vertex, orthogonal to X and Y
%
% The FTG coordinate system is defined as:
%   the origin corresponds with pt1
%   the x-axis is along the line from pt1 to pt2
%   the z-axis is orthogonal to the plane spanned by pt1, pt2 and pt3
%
% The PAXINOS coordinate system is defined as:
%   the origin is at bregma
%   the x-axis extends along the Medial-Lateral direction, with positive towards the right
%   the y-axis points from dorsal to ventral, i.e. from inferior to superior
%   the z-axis passes through bregma and lambda and points from cranial to caudal, i.e. from anterior to posterior
%
% See also FT_ELECTRODEREALIGN, FT_VOLUMEREALIGN, FT_INTERACTIVEREALIGN, FT_AFFINECOORDINATES, COORDSYS2LABEL

% Copyright (C) 2003-2022, 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$

% figure out the input arguments
if nargin==3
  coordsys = 'ctf'; % default
  fid4     = [];
elseif nargin==4 && ischar(fid4)
  coordsys = fid4;
  fid4     = [];
elseif nargin==4 && isnumeric(fid4)
  coordsys = 'ctf'; % default
elseif nargin==5
  % do nothing
else
  ft_error('incorrect specification of input parameters');
end

if isnumeric(coordsys)
  % these are for backward compatibility, but should preferably not be used any more
  if coordsys==0
    coordsys = 'ctf';
  elseif coordsys==1
    coordsys = 'asa';
  elseif coordsys==2
    coordsys = 'ftg';
  else
    ft_error('if coordsys is numeric, it should assume one of the values 0/1/2');
  end
end

% ensure that they are row vectors
fid1 = fid1(:)';
fid2 = fid2(:)';
fid3 = fid3(:)';
fid4 = fid4(:)';

assert(numel(fid1)==3, 'incorrect specification of fiducial 1');
assert(numel(fid2)==3, 'incorrect specification of fiducial 2');
assert(numel(fid3)==3, 'incorrect specification of fiducial 3');
assert(isempty(fid4) || numel(fid4)==3, 'incorrect specification of fiducial 4');

% ensure that it is lower case
coordsys = lower(coordsys);

% compute the origin and direction of the coordinate axes in MRI coordinates
switch coordsys
  case {'ctf' '4d' 'bti' 'eeglab' 'yokogawa'}
    % rename the marker points for convenience
    nas = fid1; lpa = fid2; rpa = fid3; extrapoint = fid4; clear fid*
    origin = (lpa+rpa)/2;
    dirx = nas-origin;
    dirz = cross(dirx,lpa-rpa);
    diry = cross(dirz,dirx);
    dirx = dirx/norm(dirx);
    diry = diry/norm(diry);
    dirz = dirz/norm(dirz);
  case {'asa'}
    % rename the marker points for convenience
    nas = fid1; lpa = fid2; rpa = fid3; extrapoint = fid4; clear fid*
    dirz = cross(nas-rpa, lpa-rpa);
    diry = lpa-rpa;
    dirx = cross(diry,dirz);
    dirx = dirx/norm(dirx);
    diry = diry/norm(diry);
    dirz = dirz/norm(dirz);
    origin = rpa + dot(nas-rpa,diry)*diry;
  case {'neuromag' 'itab'}
    % rename the fiducials
    nas = fid1; lpa = fid2; rpa = fid3; extrapoint = fid4; clear fid*
    dirz = cross(rpa-lpa,nas-lpa);
    dirx = rpa-lpa;
    diry = cross(dirz,dirx);
    dirx = dirx/norm(dirx);
    diry = diry/norm(diry);
    dirz = dirz/norm(dirz);
    origin = lpa + dot(nas-lpa,dirx)*dirx;
  case 'ftg'
    % rename the marker points for convenience
    pt1 = fid1; pt2 = fid2; pt3 = fid3; extrapoint = fid4; clear fid*
    origin = pt1;
    dirx = pt2-origin;
    diry = pt3-origin;
    dirz = cross(dirx,diry);
    diry = cross(dirz,dirx);
    dirx = dirx/norm(dirx);
    diry = diry/norm(diry);
    dirz = dirz/norm(dirz);
  case {'acpc' 'spm' 'mni' 'tal'}
    % rename the marker points for convenience
    ac = fid1; pc = fid2; midsagittal = fid3; extrapoint = fid4; clear fid*
    origin = ac;
    diry   = ac-pc;
    dirz   = midsagittal-ac;
    dirx   = cross(diry,dirz);
    dirz   = cross(dirx,diry);
    dirx   = dirx/norm(dirx);
    diry   = diry/norm(diry);
    dirz   = dirz/norm(dirz);
  case 'paxinos'
    bregma = fid1; lambda = fid2; midsagittal = fid3; extrapoint = fid4; clear fid*
    origin = bregma;           % bregma is slightly above the brain
    dirz = lambda-bregma;      % lambda is slightly above the brain
    diry = bregma-midsagittal; % midsagittal should be somewhere in the middle of the brain
    dirx = cross(diry,dirz);   % compute the positive x-direction, i.e. towards the right
    diry = cross(dirz,dirx);   % update the y-direction
    dirx = dirx/norm(dirx);
    diry = diry/norm(diry);
    dirz = dirz/norm(dirz);
  otherwise
    ft_error('unrecognized headcoordinate system "%s"', coordsys);
end

% use the extra point to validate that it is a right-handed coordinate system
if ~isempty(extrapoint)
  dirq = extrapoint-origin;
  dirq = dirq/norm(dirq);
  if any(strcmp(coordsys, {'ctf' '4d' 'bti' 'eeglab' 'yokogawa' 'neuromag' 'itab'}))
    phi = dirq(:)'*dirz(:);
    if sign(phi)<0
      ft_warning('the input coordinate system seems left-handed, flipping z-axis to keep the transformation matrix consistent');
      dirz = -dirz;
    end
  elseif any(strcmp(coordsys, {'acpc' 'spm' 'mni' 'tal'}))
    phi = dirq(:)'*dirx(:);
    if sign(phi)<0
      ft_warning('the input coordinate system seems left-handed, flipping x-axis to keep the transformation matrix consistent');
      dirx = -dirx;
    end
  else
    ft_warning('the extra input coordinate is not used with coordsys "%s"', coordsys);
  end
end

% compute the rotation matrix
rot = eye(4);
rot(1:3,1:3) = inv(eye(3) / [dirx; diry; dirz]);
% compute the translation matrix
tra = eye(4);
tra(1:4,4)   = [-origin(:); 1];
% combine these to compute the full homogeneous transformation matrix
transform = rot * tra;


function [s, cfg] = ft_statfun_roc(cfg, dat, design)

% FT_STATFUN_ROC computes the area under the curve (AUC) of the Receiver Operator
% Characteristic (ROC). This is a measure of the separability of the data observed in
% two conditions. The AUC can be used for statistical testing whether the two
% conditions can be distinguished on the basis of the data.
%
% Use this function by calling one of the high-level statistics functions as
%   [stat] = ft_timelockstatistics(cfg, timelock1, timelock2, ...)
%   [stat] = ft_freqstatistics(cfg, freq1, freq2, ...)
%   [stat] = ft_sourcestatistics(cfg, source1, source2, ...)
% with the following configuration option
%   cfg.statistic = 'ft_statfun_roc'
%
% The experimental design is specified as:
%   cfg.ivar  = independent variable, row number of the design that contains the labels of the conditions to be compared (default=1)
%
% The labels for the independent variable should be specified as the number 1 and 2.
%
% Note that this statfun performs a one sided test in which condition "1" is assumed
% to be larger than condition "2". This function does not compute an analytic
% probability of condition "1" being larger than condition "2", but can be used in a
% randomization test, including clustering.
%
% A low-level example with 10 channel-time-frequency points and 1000 observations per
% condition goes like this:
%   dat1 = randn(10,1000) + 1;
%   dat2 = randn(10,1000);
%   design = [1*ones(1,1000) 2*ones(1,1000)];
%   stat = ft_statfun_roc([], [dat1 dat2], design);
%
% See also FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS

% Copyright (C) 2008, 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$

% set the defaults
cfg.ivar = ft_getopt(cfg, 'ivar', 1);

% on-the-fly log transformation is not supported any more, please use FT_MATH instead
cfg = ft_checkconfig(cfg, 'forbidden', 'logtransform');
cfg = ft_checkconfig(cfg, 'forbidden', 'numbins');

% start with a quick test to see whether there appear to be NaNs
if any(isnan(dat(1,:)))
  % exclude trials that contain NaNs for all observed data points
  sel    = all(isnan(dat),1);
  dat    = dat(:,~sel);
  design = design(:,~sel);
end

% logical indexing is faster than using find(...)
selA = (design(cfg.ivar,:)==1);
selB = (design(cfg.ivar,:)==2);
% select the data in the two classes
datA = dat(:, selA);
datB = dat(:, selB);

nobs = size(dat,1);
na   = size(datA,2);
nb   = size(datB,2);
auc  = zeros(nobs, 1);

for k = 1:nobs
  % compute the area under the curve for each channel/time/frequency
  a = datA(k,:);
  b = datB(k,:);

  % to speed up the AUC, the critical value is determined by the actual
  % values in class B, which also ensures a regular sampling of the False Alarms
  b = sort(b);

  ca = zeros(nb+1,1);
  ib = zeros(nb+1,1);
  % cb = zeros(nb+1,1);
  % ia = zeros(nb+1,1);

  for i=1:nb
    % for the first approach below, the critval could also be choosen based on e.g. linspace(min,max,n)
    critval = b(i);

    % for each of the two distributions, determine the number of correct and incorrect assignments given the critical value
    % ca(i) = sum(a>=critval);
    % ib(i) = sum(b>=critval);
    % cb(i) = sum(b<critval);
    % ia(i) = sum(a<critval);

    % this is a much faster approach, which works due to using the sorted values in b as the critical values
    ca(i) = sum(a>=critval);   % correct assignments to class A
    ib(i) = nb-i+1;            % incorrect assignments to class B
  end

  % add the end point
  ca(end) = 0;
  ib(end) = 0;
  % cb(end) = nb;
  % ia(end) = na;

  hits = ca/na;
  fa   = ib/nb;

  % the numerical integration is faster if the points are sorted
  hits = fliplr(hits);
  fa   = fliplr(fa);

  if false
    % this part is optional and should only be used when exploring the data
    figure
    plot(fa, hits, '.-')
    xlabel('false positive');
    ylabel('true positive');
    title('ROC-curve');
  end

  % compute the area under the curve using numerical integration
  auc(k) = numint(fa, hits);

end

% return the area under the curve as the statistic of interest
s.stat = auc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NUMINT computes a numerical integral of a set of sampled points using
% linear interpolation. Alugh the algorithm works for irregularly sampled points
% along the x-axis, it will perform best for regularly sampled points
%
% Use as
%   z = numint(x, y)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function z = numint(x, y)
if ~all(diff(x)>=0)
  % ensure that the points are sorted along the x-axis
  [x, i] = sort(x);
  y = y(i);
end
n = length(x);
z = 0;
for i=1:(n-1)
  x0 = x(i);
  y0 = y(i);
  dx = x(i+1)-x(i);
  dy = y(i+1)-y(i);
  z = z + (y0 * dx) + (dy*dx/2);
end


function [ vX, vV, mX ] = ProjectedGdFista( vX, vV, hG, hP, numIterations, stepSize )
% ----------------------------------------------------------------------------------------------- %
% [ vX, vV, mX ] = ProjectedGdFista( vX, vV, hG, hP, numIterations, stepSize )
% Solves an objective function by the Projected Gradient Descent /
% Projected Gradient Method (PGM) with FISTA Acceleration.
% Input:
%   - vX                -   Input Vector.
%                           The starting point for the gradient iterations.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vV                -   Acceleration Vector.
%                           The buffer to hold the accelerated direction
%                           vector.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - hG                -   Gradient Function Handler.
%                           A function handler which accepts a vector of
%                           size (numElements x 1) and returns the gradient
%                           in the form of a vector (numElements x 1).
%                           Structure: Function Handler.
%                           Type: NA.
%                           Range: NA.
%   - hP                -   Gradient Function Handler.
%                           A function handler which accepts a vector of
%                           size (numElements x 1) and returns the gradient
%                           in the form of a vector (numElements x 1).
%                           Structure: Function Handler.
%                           Type: NA.
%                           Range: NA.
%   - numIterations     -   Number of Iterations.
%                           Sets the number of iterations of the gradient
%                           descent.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3, ...}.
%   - stepSize          -   Step Size.
%                           Sets the step size of the Gradient step.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
% Output:
%   - vX                -   Output Vector.
%                           The end point for the gradient iterations.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vV                -   Acceleration Vector.
%                           The buffer to hold the accelerated direction
%                           vector of last iteration. May used by a wrapper
%                           for warm start.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - mX                -   The Path Matrix.
%                           The points of the gradient iterations.
%                           Structure: Matrix (numElements x numIterations).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References:
%   1.  Geoff Gordon & Ryan Tibshirani - Accelerated First Order Methods (https://www.cs.cmu.edu/~ggordon/10725-F12/slides/09-acceleration.pdf).
% Remarks:
%   1.  B
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     26/12/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

mX = zeros(size(vX, 1), numIterations);

vG          = zeros(size(vX, 1), 1);
mX(:, 1)    = vX;
mX(:, 2)    = vX - stepSize * hG(vX);

for ii = 3:numIterations
    vV(:) = mX(:, ii - 1) + (((ii - 2) / (ii + 1)) * (mX(:, ii - 1) - mX(:, ii - 2)));
    vG(:) = hG(vV); %<! The gradient
    vX(:) = hP(vV - (stepSize * vG)); %<! Projection step

    mX(:, ii) = vX;
end

end


function [fx,tt]=v_fxrapt(s,fs,mode,q)
%V_FXRAPT RAPT pitch tracker [FX,VUV]=(S,FS,M,Q)
%
% Input:   s(ns)      Speech signal
%          fs         Sample frequency (Hz)
%          mode       'g' will plot a graph [default if no output arguments]
%                     'u' will include unvoiced fames (with fx=NaN)
%          q          stucture with parameter values (e.g. q.f0min=40); see below for a list
%
% Outputs: fx(nframe)     Larynx frequency for each fram,e (or NaN for silent/unvoiced)
%          tt(nframe,3)  Start and end samples of each frame. tt(*,3)=1 at the start of each talk spurt
%
% Plots a graph if no outputs are specified showing lag candidates and selected path
%

% Bugs/Suggestions:
%   (1) Include backward DP pass and output the true cost for each candidate.
%   (2) Add an extra state to distinguish between voiceless and silent
%   (3) N-best DP to allow longer term penalties (e.g. for frequent pitch doubling/halving)

% The algorithm is taken from [1] with the following differences:
%
%      (a)  the factor AFACT which in the Talkin algorithm corresponds roughly
%           to the absolute level of harmonic noise in the correlation window. This value
%           is here calculated as the maximum of three figures:
%                   (i) an absolute floor set by p.absnoise
%                  (ii) a multiple of the peak signal set by p.signoise
%                 (iii) a multiple of the noise floor set by p.relnoise
%      (b) The LPC used in calculating the Itakura distance uses a Hamming window rather than
%          a Hanning window.
%
% A C implementation of this algorithm by Derek Lin and David Talkin is included as  "get_f0.c"
% in the esps.zip package available from http://www.speech.kth.se/esps/esps.zip under the BSD
% license.
%
% Refs:
%      [1]   D. Talkin, "A Robust Algorithm for Pitch Tracking (RAPT)"
%            in "Speech Coding & Synthesis", W B Kleijn, K K Paliwal eds,
%            Elsevier ISBN 0444821694, 1995

%      Copyright (C) Mike Brookes 2006-2013
%      Version: $Id: v_fxrapt.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

s=s(:); % force s to be a column
if nargin<4
    q=[];
    if nargin<3
        mode=' ';
    end
end
doback=0;   % don't do backwards DP for now

% set default parameters

p0.f0min=50;           % Min F0 (Hz)
p0.f0max=500;          % Max F0 (Hz)
p0.tframe=0.01;        % frame size (s)
p0.tlpw=0.005;         % low pass filter window size (s)
p0.tcorw=0.0075;       % correlation window size (s)
p0.candtr=0.3;         % minimum peak in NCCF
p0.lagwt=0.3;          % linear lag taper factor
p0.freqwt=0.02;        % cost factor for F0 change
p0.vtranc=0.005;       % fixed voice-state transition cost
p0.vtrac=0.5;          % delta amplitude modulated transition cost
p0.vtrsc=0.5;          % delta spectrum modulated transition cost
p0.vobias=0.0;         % bias to encourage voiced hypotheses
p0.doublec=0.35;       % cost of exact doubling or halving
p0.absnoise=0;         % absolute rms noise level
p0.relnoise=2;         % rms noise level relative to noise floor
p0.signoise=0.001;     % ratio of peak signal rms to noise floor (0.001 = 60dB)
p0.ncands=20;          % max hypotheses at each frame
p0.trms=0.03;                      % window length for rms measurement
p0.dtrms=0.02;                     % window spacing for rms measurement
p0.preemph=-7000;                  % s-plane position of preemphasis zero
p0.nfullag=7;                      % number of full lags to try (must be odd)
p=v_paramsetch(p0,q);

% redefine commonly used parameters

candtr=p.candtr;          % minimum peak in NCCF                      [0.3]
vtranc=p.vtranc;          % fixed voice-state transition cost         [0.005]
vtrac=p.vtrac;            % delta amplitude modulated transition cost [0.5]
vtrsc=p.vtrsc;            % delta spectrum modulated transition cost  [0.5]
vobias=p.vobias;          % bias to encourage voiced hypotheses       [0.0]
doublec=p.doublec;        % cost of exact doubling or halving         [0.35]
ncands=p.ncands;          % max hypotheses at each frame              [20]
nfullag=p.nfullag;        % number of full lags to try (must be odd)  [7]

% derived parameters (mostly dependent on sample rate fs)

krms=round(p.trms*fs);            % window length for rms measurement
kdrms=round(p.dtrms*fs);          % window spacing for rms measurement
% rmswin=hanning(krms).^2;
rmswin=(0.5-0.5*cos(2*pi*(1:krms)'/(krms+1))).^2; % squared Hanning window
kdsmp=round(0.25*fs/p.f0max);
hlpw=round(p.tlpw*fs/2);          % force window to be an odd length
blp=sinc((-hlpw:hlpw)/kdsmp).*hamming(2*hlpw+1).';
fsd=fs/kdsmp;
kframed=round(fsd*p.tframe);      % downsampled frame length
kframe=kframed*kdsmp;           % frame increment at full rate
rmsix=(1:krms)+floor((kdrms-kframe)/2); % rms index according to Talkin; better=(1:krms)+floor((kdrms-krms+1)/2)
minlag=ceil(fsd/p.f0max);
maxlag=round(fsd/p.f0min);        % use round() only because that is what Talkin does
kcorwd=round(fsd*p.tcorw);        % downsampled correlation window
kcorw=kcorwd*kdsmp;             % full rate correlation window
spoff=max(hlpw-floor(kdsmp/2),1+kdrms-rmsix(1)-kframe);  % offset for first speech frame at full rate
sfoff=spoff-hlpw+floor(kdsmp/2); % offset for downsampling filter
sfi=1:kcorwd;                   % initial decimated correlation window index array
sfhi=1:kcorw;                   % initial correlation window index array
sfj=1:kcorwd+maxlag;
sfmi=repmat((minlag:maxlag)',1,kcorwd)+repmat(sfi,maxlag-minlag+1,1);
lagoff=(minlag-1)*kdsmp;        % lag offset when converting to high sample rate
beta=p.lagwt*p.f0min/fs;            % bias towards low lags
log2=log(2);
lpcord=2+round(fs/1000);        % lpc order for itakura distance
hnfullag=floor(nfullag/2);
jumprat=exp((doublec+log2)/2);  % lag ratio at which octave jump cost is lowest
ssq=s.^2;
csssq=cumsum(ssq);
sqrt(min(csssq(kcorw+1:end)-csssq(1:end-kcorw))/kcorw);
afact=max([p.absnoise^2,max(ssq)*p.signoise^2,min(csssq(kcorw+1:end)-csssq(1:end-kcorw))*(p.relnoise/kcorw)^2])^2*kcorw^2;

% downsample signal to approx 2 kHz to speed up autocorrelation calculation
% kdsmp is the downsample factor

sf=filter(blp/sum(blp),1,s(sfoff+1:end));
sp=filter([1 exp(p.preemph/fs)],1,s); % preemphasised speech for LPC calculation
sf(1:length(blp)-1)=[];         % remove startup transient
sf=sf(1:kdsmp:end);             % downsample to =~2kHz
nsf=length(sf);                 % length of downsampled speech
ns=length(s);                   % length of full rate speech

% Calculate the frame limit to ensure we don't run off the end of the speech or decimated speech:
%   (a) For decimated autocorrelation when calculating sff():  (nframe-1)*kframed+kcorwd+maxlag <= nsf
%   (b) For full rate autocorrelation when calculating sfh():  max(fho)+kcorw+maxlag*kdsamp+hnfllag <= ns
%   (c) For rms ratio window when calculating rr            :  max(fho)+rmsix(end) <= ns
% where max(fho) = (nframe-1)*kframe + spoff

nframe=floor(1+min((nsf-kcorwd-maxlag)/kframed,(ns-spoff-max(kcorw-maxlag*kdsmp-hnfullag,rmsix(end)))/kframe));

% now search for autocorrelation peaks in the downsampled signal

cost=zeros(nframe,ncands);      % cumulative cost
prev=zeros(nframe,ncands);      % traceback pointer
mcands=zeros(nframe,1);         % number of actual candidates excluding voiceless
lagval=repmat(NaN,nframe,ncands-1);    % lag of each voiced candidate
tv=zeros(nframe,3);             % diagnostics: 1=voiceless cost, 2=min voiced cost, 3:cumulative voiceless-min voiced
if doback
    costms=cell(nframe,1);
end

% Main processing loop for each 10 ms frame

for iframe=1:nframe       % loop for each frame (~10 ms)

    % Find peaks in the normalized autocorrelation of subsampled (2Khz) speech
    % only keep peaks that are > 30% of highest peak

    fho=(iframe-1)*kframe+spoff;
    sff=sf((iframe-1)*kframed+sfj);
    sffdc=mean(sff(sfi));       % mean of initial correlation window length
    sff=sff-sffdc;              % subtract off the mean
    nccfd=normxcor(sff(1:kcorwd),sff(minlag+1:end));
    [ipkd,vpkd]=v_findpeaks(nccfd,'q');

    % Debugging: execute the line below to plot the autocorrelation peaks.
    % v_findpeaks(nccfd,'q'); xlabel(sprintf('Lag = (x+%d)*%g ms',minlag-1,1000*kdsmp/fs)); ylabel('Normalized Cross Correlation'); title (sprintf('Frame %d/%d',iframe,nframe));

    vipkd=[vpkd ipkd];
    vipkd(vpkd<max(vpkd)*candtr,:)=[];          % eliminate peaks that are small
    if size(vipkd,1)
        if size(vipkd,1)>ncands-1
            vipkd=sortrows(vipkd);
            vipkd(1:size(vipkd,1)-ncands+1,:)=[];   % eliminate lowest to leave only ncands-1
        end
        lagcan=round(vipkd(:,2)*kdsmp+lagoff);        % convert the lag candidate values to the full sample rate
        nlcan=length(lagcan);
    else
        nlcan=0;
    end

    % If there are any candidate lag values (nlcan>0) then refine their accuracy at the full sample rate

    if nlcan
        laglist=reshape(repmat(lagcan(:)',nfullag,1)+repmat((-hnfullag:hnfullag)',1,nlcan),nfullag*nlcan,1);
        sfh=s(fho+(1:kcorw+max(lagcan)+hnfullag));
        sfhdc=mean(sfh(sfhi));
        sfh=sfh-sfhdc;
        e0=sum(sfh(sfhi).^2);                     % energy of initial correlation window (only needed to store in tv(:,6)
        lagl2=repmat(lagcan(:)',nfullag+kcorw-1,1)+repmat((1-hnfullag:hnfullag+kcorw)',1,nlcan);
        nccf=normxcor(sfh(1:kcorw),sfh(lagl2),afact);

        [maxcc,maxcci]=max(nccf,[],1);
        vipk=[maxcc(:) lagcan(:)+maxcci(:)-hnfullag-1];
        vipk=vipk(:,[1 2 2]);
        maxccj=maxcci(:)'+nfullag*(0:nlcan-1);    % vector index into nccf array
        msk=mod(maxcci,nfullag-1)~=1 & 2*nccf(maxccj)-nccf(mod(maxccj-2,nfullag*nlcan)+1)-nccf(mod(maxccj,nfullag*nlcan)+1)>0;  % don't do quadratic interpolation for the end ones
        if any(msk)
            maxccj=maxccj(msk);
            vipk(msk,3)=vipk(msk,3)+(nccf(maxccj+1)-nccf(maxccj-1))'./(2*(2*nccf(maxccj)-nccf(maxccj-1)-nccf(maxccj+1)))';
        end
        vipk(maxcc<max(maxcc)*candtr,:)=[];          % eliminate peaks that are small
        if size(vipk,1)>ncands-1
            vipk=sortrows(vipk);
            vipk(1:size(vipk,1)-ncands+1,:)=[];   % eliminate lowest to leave only ncands-1
        end

        % vipk(:,1) has NCCF value, vipk(:,2) has integer peak position, vipk(:,3) has refined peak position

        mc=size(vipk,1);
    else
        mc=0;
    end

    % We now have mc lag candidates at the full sample rate

    mc1=mc+1;               % total number of candidates including "unvoiced" possibility
    mcands(iframe)=mc;      % save number of lag candidates (needed for pitch consistency cost calculation)
    if mc
        lagval(iframe,1:mc)=vipk(:,3)';
        cost(iframe,1)=vobias+max(vipk(:,1));   % voiceless cost
        cost(iframe,2:mc1)=1-vipk(:,1)'.*(1-beta*vipk(:,3)');   % local voiced costs
        tv(iframe,2)=min(cost(iframe,2:mc1));
    else
        cost(iframe,1)=vobias;          % if no lag candidates (mc=0), then the voiceless case is the only possibility
    end
    tv(iframe,1)=cost(iframe,1);
    if iframe>1                         % if it is not the first frame, then calculate pitch consistency and v/uv transition costs
        mcp=mcands(iframe-1);
        costm=zeros(mcp+1,mc1);         % cost matrix: rows and cols correspond to candidates in previous and current frames (incl voiceless)

        % if both frames have at least one lag candidate, then calculate a pitch consistency cost

        if mc*mcp
            lrat=abs(log(repmat(lagval(iframe,1:mc),mcp,1)./repmat(lagval(iframe-1,1:mcp)',1,mc)));
            costm(2:end,2:end)=p.freqwt*min(lrat,doublec+abs(lrat-log2));  % allow pitch doubling/halving
        end

        % if either frame has a lag candidate, then calculate the cost of voiced/voiceless transition and vice versa

        if mc+mcp
            rr=sqrt((rmswin'*s(fho+rmsix).^2)/(rmswin'*s(fho+rmsix-kdrms).^2)); % amplitude "gradient"
            ss=0.2/(v_distitar(v_lpcauto(sp(fho+rmsix),lpcord),v_lpcauto(sp(fho+rmsix-kdrms),lpcord),'e')-0.8);   % Spectral stationarity: note: Talkin uses Hanning instead of Hamming windows for LPC
            costm(1,2:end)= vtranc+vtrsc*ss+vtrac/rr;   % voiceless -> voiced cost
            costm(2:end,1)= vtranc+vtrsc*ss+vtrac*rr;
            tv(iframe,4:5)=[costm(1,mc1) costm(mcp+1,1)];
        end
        costm=costm+repmat(cost(iframe-1,1:mcp+1)',1,mc1);  % add in cumulative costs
        [costi,previ]=min(costm,[],1);
        cost(iframe,1:mc1)=cost(iframe,1:mc1)+costi;
        prev(iframe,1:mc1)=previ;
    else                            % first ever frame
        costm=zeros(1,mc1); % create a cost matrix in case doing a backward recursion
    end
    if mc
        tv(iframe,3)=cost(iframe,1)-min(cost(iframe,2:mc1));
        tv(iframe,6)=5*log10(e0*e0/afact);
    end
    if doback
        costms{iframe}=costm; % need to add repmatted cost into this
    end
end

% now do traceback

best=zeros(nframe,1);
[cbest,best(nframe)]=min(cost(nframe,1:mcands(nframe)+1));
for i=nframe:-1:2
    best(i-1)=prev(i,best(i));
end
vix=find(best>1);
fx=repmat(NaN,nframe,1);                        % unvoiced frames will be NaN
fx(vix)=fs*lagval(vix+nframe*(best(vix)-2)).^(-1); % leave as NaN if unvoiced
tt=zeros(nframe,3);
tt(:,1)=(1:nframe)'*kframe+spoff;       % find frame times
tt(:,2)=tt(:,1)+kframe-1;
jratm=(jumprat+1/jumprat)/2;
tt(2:end,3)=abs(fx(2:end)./fx(1:end-1)-jratm)>jumprat-jratm;    % new spurt if frequency ratio is outside (1/jumprat,jumprat)
tt(1,3)=1;           % first frame always starts a spurt
tt(1+find(isnan(fx(1:end-1))),3)=1; % NaN always forces a new spurt

% plot results if there are no output arguments of if the 'g' mode option is specified

if ~nargout || any(mode=='g')
    tf=spoff+(0:nframe-1)'*kframe;      % one sample before start of each frame
    blag=repmat(NaN,nframe,1);                        % unvoiced frames will be NaN
    blag(vix)=lagval(vix+nframe*(best(vix)-2)); % leave as NaN if unvoiced
    ts=(1:ns)/fs;                       % time scale for speech samples
    tsa=[1:tf(1) tf(end)+kframe+1:ns];  % indexes for unprocessed speech [-1 term is an error methinks]
    sup=repmat(NaN,ns,1);               % unprocessed speech - plot in black
    sup(tsa)=s(tsa);
    sv=reshape(s(tf(1)+1:tf(end)+kframe),kframe,nframe);               % processed speech
    su=sv;
    su(:,best>1)=NaN;                   % delete all voiced samples
    sv(:,best==1)=NaN;                  % delete all unvoiced samples
    tsuv=(tf(1)+1:tf(end)+kframe)/fs;
    su=su(:);
    sv=sv(:);
    ax=zeros(2,1);
    ax(1)=subplot(211);
    plot(ts,sup,'-k',tsuv,su,'r-',tsuv,sv,'b-');
    title('Speech');
    ax(2)=subplot(212);
    plot((tf+(kframe+1)/2)/fs,lagval*1000/fs,'xr',(tf+(kframe+1)/2)/fs,blag*1000/fs,'-b')
    xlabel('Time (s)');
    ylabel('Period (ms)');
    title('Lag Candidates');
    linkaxes(ax,'x');
end
if ~any(mode=='u')
    tt(isnan(fx),:)=[];    % remove NaN spurts
    fx(isnan(fx),:)=[];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function v=normxcor(x,y,d)
% Calculate the normalized cross correlation of column vectors x and y
% we can calculate this in two ways but fft is much faster even for nx small
% We must have nx<=ny and the output length is ny-nx+1
% note that this routine does not do mean subtraction even though this is normally a good idea
% if y is a matrix, we correlate with each column
% d is a constant added onto the normalization factor
% v(j)=x'*yj/sqrt(d + x'*x * yj'*yj) where yj=y(j:j+nx-1) for j=1:ny-nx+1

if nargin<3
    d=0;
end
nx=length(x);
[ny,my]=size(y);
nv=1+ny-nx;
if nx>ny
    error('second argument is shorter than the first');
end

nf=pow2(nextpow2(ny));
w=v_irfft(repmat(conj(v_rfft(x,nf,1)),1,my).*v_rfft(y,nf,1));
s=zeros(ny+1,my);
s(2:end,:)=cumsum(y.^2,1);
v=w(1:nv,:)./sqrt(d+(x'*x).*(s(nx+1:end,:)-s(1:end-nx,:)));