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 [ PH CH colorScale] = plotSeries(varargin)
%
% [ PH CH colorList ] = plotSeries(AH,x,y,s,colormap,OPTIONS)
%
% Plots x and y data with each column of y color-coded according to scalar
% values defined in s.  plotSeries is particularly useful to illustrate how
% a x-y dataset *changes* in response to some value, s.
%
% Colormap can be defined by a string or a four-column matrix.  When a string
% is used or the argument is left empty, the colormap spans all values of
% s, linearly.  When a four-column matrix is provided, the first column
% corresponds to potential values of s, the other three define the corresponding
% RGB values.  The RGB values used for each column of y are interpolated
% from the rows of colormap, so there is no requirement for the number of
% rows of colormap to match up w/ the length of s.
%
% If length(s) > 1, then a colorbar will be included as a legend for the
% line coloring.
%
% Optional lineseries specifications (OPTIONS) will be applied to all
% plots.
%
% plotSeries outputs a list of plotHandles (PH), a colorbar handle, (CH),
% and the colorList used for the plot.  The colorList output can be used
% directly as the colormap argument for other calls to plotSeries,
% providing a mechanism to easily produce multiple plots using similar
% coloring schemes.
%
%  Example (cut and paste into the command-line):
%
%  % Show how sinc function changes as a function of s
%  s = 1:10;
%  x = (-1:1e-3:1).';
%  y = sinc(x*s);
%  figure; plotSeries(x,y,s,'jet','LineWidth',2);
%  xlabel('x-values');ylabel('y-values');
%
%  Inputs:  AH         - [1x1]       [OPTIONAL]  Axes handle
%           x          - [NxS / Nx1] X-coordinate data
%           y          - [NxS]       Y-coordinate data
%           s          - [1xS]       Scalar value associated with each data series
%                                    (length(s) = size(y,2))
%           colormap   - [Cx4]       Colormap to use for dataset coloring.
%                                    Can be colormap string OR four-column
%                                    colormap with 1st column corresponding to
%                                    potential s values and 3 remaining columns
%                                    corresponding to color.  Note that the
%                                    colormap will be interpolated to find
%                                    the appropriate color for each
%                                    dataset. ([] for 'default' colormap)
%           OPTIONS    -             Optional arguments for plot command
%
%  Outputs: PH         - [Sx1] Plot handle
%           CH         - [1x1] Colorbar handle
%           colorScale - [Sx4] Colormap w/ 1st column corresponding to
%                              s values and 3 remaining columns corresponding to color
%
%  N - Number of x-y coordinates to plot for each dataset
%  S - Number of datasets
%  C - Number of colors provided in colormap
%
% Copyright (c) 2011, Hidden Solutions, LLC
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
%    * Redistributions of source code must retain the above copyright
%      notice, this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright
%      notice, this list of conditions and the following disclaimer in the
%      documentation and/or other materials provided with the distribution.
%    * Neither the name Hidden Solutions, LLC nor the names of any
%      contributors may be used to endorse or promote products derived from
%      this software without specific prior written permission.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
% DISCLAIMED. IN NO EVENT SHALL HIDDEN SOLUTIONS, LLC BE LIABLE FOR ANY
% DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
% (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
% LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
% ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
% (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
% Author: James S. Hall (Hidden Solutions, LLC)
% Date:   17 October 2011
%

% Determine axes handle
if numel(varargin{1}) == 1 && ishandle(varargin{1})
  % If provided, pop AH from list of arguments
  AH       = varargin{1};
  varargin = varargin(2:end);
else
  % Assign current axes
  AH = gca;
end

% Assign x,y,z arguments
x = varargin{1};
y = varargin{2};
z = varargin{3};

% Force to column vectors (if appropriate)
if size(y,1) == 1
  y = y(:);
end
if size(z,1) == 1
  z = z(:);
end

% Generate colorList for plots
if length(varargin)>3 && ~isempty(varargin{4})
  if ischar(varargin{4})
    % If string provided, use as name of colormap
    cmap       = colormap(varargin{4});
    colorScale = [((0:1/(size(cmap,1)-1):1).'*range(z))+min(z) cmap];
  elseif size(varargin{4},2) == 4
    % Assign colormap to axes
    colorScale = varargin{4};
  end
  % Assign colormap to axes
  colormap(AH,colorScale(:,2:4));
else
  % Use default colormap
  cmap       = colormap();
  colorScale = [((0:1/(size(cmap,1)-1):1).'*range(z))+min(z) cmap];
end

% Determine colors to use for each plot
if length(z) > 1
  % Sort colors for interp1
  [cSort cInd] = sort(colorScale(:,1));
  colorScale   = colorScale(cInd,:);
  colorList    = interp1(colorScale(:,1),colorScale(:,2:4),z,'linear',nan);
  % Force s values outside of colorScale(:,1) range to max/min colorScale
  colorList(isnan(colorList(:,1))&z<min(colorScale(:,1)),:) = repmat(colorScale(1,:),sum(isnan(colorList(:,1))&z<min(colorScale(:,1))),1);
  colorList(isnan(colorList(:,1))&z>max(colorScale(:,1)),:) = repmat(colorScale(end,:),sum(isnan(colorList(:,1))&z<min(colorScale(:,1))),1);
else
  % Only one s value provided, use for all plots
  colorList = repmat(colorScale(1,2:4),size(y,2),1);
end

% Plot data
PH = plot(AH,x,y,varargin{5:end});

% Color-code data
for pp = 1:length(PH)
  set(PH(pp),'Color',colorList(pp,:));
end

if length(z) > 1
  % Add colorbar
  caxis([min(z) max(z)]);
  CH = colorbar;
end

end


function [accuracy, frames] = estimate_accuracy(trajectory, sequence, varargin)
% estimate_accuracy Calculate accuracy score
%
% Calculate accuracy score as average overlap over the entire sequence.
%
% Input:
% - trajectory (cell): A trajectory as a cell array of regions.
% - sequence (cell or structure): Can be another trajectory or a valid sequence descriptor.
% - varargin[Burnin] (integer): Number of frames that have to be ignored after the failure.
% - varargin[IgnoreUnknown] (boolean): Ignore frames where the overlap is
% unknown.
% - varargin[BindWithin] (boolean or vector): Bind the overlap calculation to the region
% within the image. If the sequence variable is a sequence structure then a boolean value
% is sufficient to establishe bounding region. Otherwise a bounding region has to be specified
% manually.
%
% Output:
% - accuracy (double): Average overlap.
% - frames (double vector): Per-frame overlaps.
%

ignore_unknown = true;
burnin = 0;
bind_within = get_global_variable('bounded_overlap', true);

for j=1:2:length(varargin)
    switch lower(varargin{j})
        case 'burnin', burnin = max(0, varargin{j+1});
        case 'ignoreunknown', ignore_unknown = varargin{j+1};
        case 'bindwithin', bind_within = varargin{j+1};
        otherwise, error(['unrecognized argument ' varargin{j}]);
    end
end

if burnin > 0

    mask = cellfun(@(r) numel(r) == 1 && r == 1, trajectory, 'UniformOutput', true);

    if is_octave()
        se = logical([zeros(burnin - 1, 1); ones(burnin, 1)]);
    else
        se = strel('arbitrary', [zeros(burnin - 1, 1); ones(burnin, 1)]);
    end;

    % ignore the next 'burnin' frames
    mask = imdilate(mask, se);

else

    mask = false(size(trajectory, 1), 1);

end;

if ~ignore_unknown
    unknown = cellfun(@(r) numel(r) == 1 && r == 0, trajectory, 'UniformOutput', true);
end;

trajectory(mask) = {0};

if islogical(bind_within)
    if bind_within && isstruct(sequence)
        bounds = [sequence.width, sequence.height] - 1;
    else
        bounds = [];
    end;
else
    bounds = bind_within;
end;

if isstruct(sequence)
    frames = calculate_overlap(trajectory, sequence_get_region(sequence, 1:sequence.length), bounds);
else
    frames = calculate_overlap(trajectory, sequence, bounds);
end;

if ~ignore_unknown
    frames(unknown) = 0;
end;

overlap = frames(~isnan(frames)); % filter-out illegal values

% Handle cases, where no overlap is available
if isempty(overlap)
    accuracy = 0;
else
    accuracy = mean(overlap);
end;


function [A, a, B, b] = adjacencyAndCoords(Lmk,Frm,Fac)

nframes  = sum([Frm.used]);
nlmks    = sum([Lmk.used]);

% Motion
A = sparse(nframes,nframes);
a = zeros(nframes,3);

% Measurements
B = sparse(nframes+nlmks,nframes+nlmks);
b = zeros(nframes+nlmks,3);

for fac = find([Fac.used])

    frames = Fac(fac).frames;
    lmk = Fac(fac).lmk;

    switch Fac(fac).type
        case'motion'
            A(frames(1),frames(2))   = 1;
            a(frames(1),1:3) = Frm(frames(1)).state.x(1:3)';
            a(frames(2),1:3) = Frm(frames(2)).state.x(1:3)';
        case 'absolute'
        case 'measurement'
            B(frames,lmk+nframes) = 1;
            b(frames,1:3) = Frm(frames).state.x(1:3)';
            b(lmk+nframes,1:3) = Lmk(lmk).state.x(1:3)';
    end

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function K = K_all2K(K_all)
%% Transform between K and K_all

M = size(K_all,2);
K = zeros(1,M);
for j = 1:M
    K(j) = max(K_all(:,j));
end


function binary( varargin )

%BINARY Declares an binary variable.
%   BINARY VARIABLE x
%   where x is a valid MATLAB variable name, declares a scalar variable for
%   the current CVX problem, and constrains it to the set {0,1}. Put
%   another way, it is equivalent to
%       integer variable x
%       0 <= x <= 1
%
%   BINARY VARIABLE x(SZ), where SZ is a size vector, declares an array of
%   size SZ and constrains each element to be binary. Structure modifiers
%   such as "symmetric", "toeplitz", etc. are also permitted.
%
%   BINARY VARIABLES x y(SZ) z ... can be used to declare multiple binary
%   variables. Note however that structure modifiers are not permitted in
%   this case.
%
%   Obviously, binary variables are NOT convex. Problems with binary
%   variables can only be handled by solvers with explicit support for
%   them; in particular, neither of the free solvers SeDuMi nor SDPT3
%   provide binary support.
%
%   Examples:
%      binary variable x
%      binary variable x(100)
%
%   See also INTEGER, VARIABLE, VARIABLES.

if nargin < 2,
    error( 'Incorrect syntax for BINARY VARIABLE(S). Type HELP BINARY for details.' );
elseif ~iscellstr( varargin ),
    error( 'All arguments must be strings.' );
elseif strcmp( varargin{1}, 'variable' ),
    evalin( 'caller', sprintf( '%s ', 'variable', varargin{2:end}, 'binary' ) );
elseif strcmp( varargin{1}, 'variables' ),
    for k = 2 : nargin,
        evalin( 'caller', sprintf( '%s ', 'variable', varargin{k}, 'binary' ) );
    end
else
    error( 'Incorrect syntax for BINARY VARIABLE(S). Type HELP BINARY for details.' );
end

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


function hh = quadmesh(quad,x,y,z,varargin)
%QUADMESH Quadrilateral mesh plot.
%   QUADMESH(QUAD,X,Y,Z,C) displays the quadrilaterals defined in the M-by-4
%   face matrix QUAD as a mesh.  A row of QUAD contains indexes into
%   the X,Y, and Z vertex vectors to define a single quadrilateral face.
%   The edge color is defined by the vector C.
%
%   QUADMESH(QUAD,X,Y,Z) uses C = Z, so color is proportional to surface
%   height.
%
%   QUADMESH(TRI,X,Y) displays the quadrilaterals in a 2-d plot.
%
%   H = QUADMESH(...) returns a handle to the displayed quadrilaterals.
%
%   QUADMESH(...,'param','value','param','value'...) allows additional
%   patch param/value pairs to be used when creating the patch object.
%
%   See also PATCH.
%
% Script code based on copyrighted code from mathworks for TRIMESH.
% Allan P. Engsig-Karup, apek@mek.dtu.dk.

ax = axescheck(varargin{:});
ax = newplot(ax);

if nargin == 3 || (nargin > 4 && ischar(z))
  d = tri(:,[1 2 3 4 1])';
  if nargin == 3
    h = plot(ax, x(d), y(d));
  else
    h = plot(ax, x(d), y(d),z,varargin{1},varargin{2:end});
  end
  if nargout == 1, hh = h; end
  return;
end

start = 1;
if nargin>4 && rem(nargin-4,2)==1
  c = varargin{1};
  start = 2;
elseif nargin<3
  error(id('NotEnoughInputs'),'Not enough input arguments');
else
  c = z;
end

if ischar(get(ax,'color')),
  fc = get(gcf,'Color');
else
  fc = get(ax,'color');
end

h = patch('faces',quad,'vertices',[x(:) y(:) z(:)],'facevertexcdata',c(:),...
	  'facecolor',fc,'edgecolor',get(ax,'defaultsurfacefacecolor'),...
	  'facelighting', 'none', 'edgelighting', 'flat',...
      'parent',ax,...
	  varargin{start:end});
if ~ishold(ax), view(ax,3), grid(ax,'on'), end
if nargout == 1, hh = h; end

function str = id(str)
str = ['MATLAB:quadmesh:' str];

return

% % Example: plot quadmesh
% close all, clear all, clc
%
% x = [0 1 2 2 1 1 0 0];
% y = [0 0 0 1 1 2 2 1];
% for i = 1 : length(x)
%     text(x(i)+0.1,y(i)+0.1,sprintf('%d',i))
%     hold on
% end
% quad = [1 2 5 8;
%     2 3 4 5;
%     8 5 6 7];
%
% plot(x,y,'k.-')
% axis off
%
% z = sin(pi*x).*cos(pi*y);
% c = z;
% figure
%
% ax=newplot;
% fc = get(gcf,'Color');
% h = patch('faces',quad,'vertices',[x(:) y(:) z(:)],'facevertexcdata',c(:),...
%     'facecolor',fc,'edgecolor',get(ax,'defaultsurfacefacecolor'),...
%     'facelighting', 'none', 'edgelighting', 'flat',...
%     'parent',ax);


function [bndinfo2, err] = transferSuperpixelLabels(bndinfo, wseg2)

wseg1 = bndinfo.wseg;

labim = zeros(size(wseg1));
labim(wseg1>0) = bndinfo.labels(wseg1(wseg1>0));

stats = regionprops(wseg2, 'PixelIdxList');
spind = {stats(:).PixelIdxList};

lab2 = zeros(max(wseg2(:)), 1);

for k = 1:numel(lab2)
    pixlab = labim(spind{k});
    pixlab = pixlab(pixlab>0);
    if numel(pixlab)>numel(spind{k})*0.01
        lab2(k) = mode(pixlab);
    end
end

bndinfo2 = bndinfo;
bndinfo2.wseg = uint16(wseg2);
bndinfo2.labels = lab2;

labim2 = zeros(size(wseg2));
labim2(wseg2>0) = bndinfo2.labels(wseg2(wseg2>0));

ind = (labim2 > 0) & (labim > 0);

if nargout > 1
    err = mean(labim(ind)~=labim2(ind));
    disp(num2str([max(wseg1(:)) max(wseg2(:))]))
    disp(['Error: ' num2str(err)]);
end


function msg = apm_arx(p,m,ny,nu,alpha,beta,gamma,name)

if nargin <= 6,
    error('apm_arx: Input ARX Model')
end
if nargin == 7,
    name = 'arx';
end
if nargin == 8,
    % trim whitespace
    filename = strtrim(name);
    % parse file name and extension
    [path,name,ext] = fileparts(name);
end
filename = [name '.apm'];

% check sizes for alpha
[alpha_ny,alpha_p] = size(alpha);
if (alpha_ny~=ny),
    disp(['input ny: ', num2str(ny)])
    disp(['alpha ny: ', num2str(alpha_ny)])
    error('ARX Size mismatch for alpha')
end
if (alpha_p~=p),
    disp(['input p: ', num2str(p)])
    disp(['alpha p: ', num2str(alpha_p)])
    error('ARX Size mismatch for alpha')
end

% check sizes for beta
[beta_nu,beta_m,beta_p] = size(beta);
if (beta_m~=m),
    disp(['input m: ', num2str(m)])
    disp(['beta m: ', num2str(beta_m)])
    error('ARX Size mismatch for beta')
end
if (beta_nu~=nu),
    disp(['input nu: ', num2str(nu)])
    disp(['beta nu: ', num2str(beta_nu)])
    error('ARX Size mismatch for beta')
end
if (beta_p~=p),
    disp(['input p: ', num2str(p)])
    disp(['beta p: ', num2str(beta_p)])
    error('ARX Size mismatch for alpha')
end

fid = fopen(filename,'w');
fprintf( fid,'\n');
fprintf( fid,'Objects \n');
fprintf( fid,['  ' name ' = arx\n']);
fprintf( fid,'End Objects \n');
fprintf( fid,'\n');
fprintf( fid,'Connections\n');
fprintf( fid,['  u[1:%d] = ' name '.u[1:%d]\n'],m,m);
fprintf( fid,['  y[1:%d] = ' name '.y[1:%d]\n'],p,p);
fprintf( fid,'End Connections\n');
fprintf( fid,'\n');
fprintf( fid,'Model \n');
fprintf( fid,'  Parameters \n');
if (m==1)
    fprintf( fid,'    u = 0\n');
else
    fprintf( fid,'    u[1:%d] = 0\n',m);
end
fprintf( fid,'  End Parameters \n');
fprintf( fid,'\n');
fprintf( fid,'  Variables \n');
if (p==1)
    fprintf( fid,'    y = 0\n');
else
    fprintf( fid,'    y[1:%d] = 0\n',p);
end
fprintf( fid,'  End Variables \n');
fprintf( fid,'\n');
fprintf( fid,'  Equations \n');
fprintf( fid,'    ! add any additional equations here \n');
fprintf( fid,'  End Equations \n');
fprintf( fid,'End Model \n');
fprintf( fid,'\n');
fprintf( fid,['File ' name '.txt\n']);
fprintf( fid,'  %d      ! m=number of inputs\n',m);
fprintf( fid,'  %d      ! p=number of outputs\n',p);
fprintf( fid,'  %d      ! nu=number of input terms\n',nu);
fprintf( fid,'  %d      ! ny=number of output terms\n',ny);
fprintf( fid,'End File\n');
fprintf( fid,'\n');
fprintf( fid,'! Alpha matrix (ny x p)\n');
fprintf( fid,['File ' name '.alpha.txt \n']);
for i = 1:ny
    for j = 1:p
        fprintf( fid,'  ');
        if j<=p-1
            fprintf( fid,'%d , ', alpha(i,j));
        else
            fprintf( fid,'%d ', alpha(i,j)); % no comma
        end
    end
    fprintf( fid,'\n');
end
fprintf( fid,'End File \n');
fprintf( fid,'\n');
fprintf( fid,'! Beta matrix (p x (nu x m))\n');
fprintf( fid,['File ' name '.beta.txt \n']);
for i = 1:p
    for j = 1:nu
        fprintf( fid,'  ');
        for k = 1:m
            if k<=m-1
                fprintf( fid,'%d , ', beta(j,k,i));
            else
                fprintf( fid,'%d ', beta(j,k,i));
            end
        end
        fprintf( fid,'\n');
    end
end
fprintf( fid,'End File \n');
fprintf( fid,'\n');
fprintf( fid,'! Gamma vector (p x 1)\n');
fprintf( fid,['File ' name '.gamma.txt \n']);
for i = 1:p
    fprintf( fid,'%d ', gamma(i));
    fprintf( fid,'\n');
end
fprintf( fid,'End File \n');
fclose(fid);

msg = ['Created ARX (Auto-Regressive eXogenous inputs) Model: ' filename];

end


function idx = sortConditions(funIn, domain, maxDiffOrders)
%SORTCONDITIONS   Returns how the results of evaluating BCs should be sorted.
%   Calling sequence:
%      IDX = SORTCONDITIONS(FUNIN, DOMAIN, DIFFORDERS)
%   where the inputs are:
%      FUNIN:         An anonymous function, that usually specifies N.LBC or
%                     N.RBC of a CHEBOP.
%      DOMAIN:        The domain of the problem.
%      MAXDIFFORDERS: The highest order of derivatives which each derivative
%                     appears in a problem.
%   and the output is
%      IDX:    A vector with indices on how to sort the results of evaluating
%              N.LBC/RBC, so that they match the order required by the MATLAB
%              ODE solvers.
%
%   Example:
%      Assume that for a coupled system, we want to specify the ICs
%         u(0) = 1, u'(0) = 2, v(0) = 0, v'(0) = 3.
%      One anonymous function we could create to specify those conditions and
%      append to a CHEBOP is
%         fun = @(u,v) [u-1; diff(v)-3; v; diff(u)-2];
%      However, when we evaluate this function to pick out the initial
%      conditions, we get the vector
%         [1; 3; 0; 2]
%      which is in the incorrect order to be passed to the MATLAB solvers, which
%      require the conditions to be of the order u, u', v, v'. Calling
%      sortConditions(),
%         idx = treeVar.sortConditions(fun, dom)
%         idx =
%             1     4     3     2
%      gives the correct order in which the above vector has to be sorted so
%      that the values are in the correct order for MATLAB.

% Check how many unknowns appear in FUNIN. When BCs are assigned via e.g.
% N.lbc = [1;3], nargin(funIn) == -1 due to the function that gets assigned
% under the hood having varargin as its argument. Hence we look at the length of
% diffOrders as well.
numArgs = max(nargin(funIn), length(maxDiffOrders));
args = cell(numArgs, 1);

% The ID vector to be passed to the TREEVAR constructor.
argsVec = zeros(1, numArgs);

% Populate the args cell with TREEVAR objects.
for argCount = 1:numArgs
    % Set the ID of the current variable to 1:
    argsVec(argCount) = 1;
    % Construct the TREEVAR:
    args{argCount} = treeVar(argsVec, domain);
    % Reset the index vector:
    argsVec = 0*argsVec;
end

% Evaluate FUNIN with the TREEVAR arguments. If we're working with the
% CHEBMATRIX syntax, we should not expand the ARGS cell (but do it otherwise):
if ( ( nargin(funIn) == 1 ) && ( length(args) > 1 ) )
    % CHEBMATRIX syntax in specifying BCs:
    bcResults = funIn(args);
else
    bcResults = funIn(args{:});
end
% Look at the results of evaluating the boundary conditions, find what
% constraint operated on what variable, and what its diffOrder was:
varList = cell(numArgs, 1);
diffOrderList = varList;

% Loop through the resulting syntax trees.
for tCounter = 1:length(bcResults)
    % Current tree we're looking at:
    tempTree = bcResults(tCounter).tree;

    % Check whether more than one variable appear in the condition, which we
    % don't support:
    assert( sum(tempTree.ID) <= 1, ...
        'CHEBFUN:TREEVAR:sortConditions:nonSeparated', ...
        'For initial value problems, only separated conditions are supported.')

    % Ensure that only initial/final conditions on the form we support appear:
    assert(acceptedCondition(tempTree), ...
        'CHEBFUN:TREEVAR:sortConditions:unsupportedCondition', [ ...
        'Initial/final condition not supported. Please ensure that the\n' ...
        'unknown function(s) appear only once in each condition,\n' ...
        'e.g. not as u + u'' - 1, and that the coefficient of the unknown\n'...
        'function is 1, e.g. not 5*u-1. Alternatively, try solving the\n' ...
        'problem in collocation mode (see help cheboppref for details).']);

    % What's the active variable in the current tree (i.e. what variable did the
    % constraint apply to)?
    activeVar = find(tempTree.ID == 1);

    % What's the diffOrder of the current constraint?
    activeDiffOrder = tempTree.diffOrder;
    activeDiffOrder = activeDiffOrder(activeVar);

    % Store in a list what variable the current constraint applies to, and what
    % the current diffOrder is:
    varList{activeVar} = [ varList{activeVar}, tCounter ];
    diffOrderList{activeVar} = [diffOrderList{activeVar}, ...
        activeDiffOrder];
end

% Initalise an index vector to be returned
idx = [];

% Go through the list of what variables appeared in what constraints, and sort
% them based on diffOrders:
for varCounter = 1:numArgs
    [varDiffOrders, diffOrderIndex] = sort(diffOrderList{varCounter});

    % Check that the user is not specifying initial values for too high order
    % derivatives.
    assert( varDiffOrders(end) < maxDiffOrders(varCounter), ...
        'CHEBFUN:TREEVAR:sortConditions:tooHighOrderCondition', ...
        'The value of a derivative of a too high order was specified.')

    % Check that multiple values for the same derivative value was not
    % specified:
    assert( length(varDiffOrders) == 1 || ~(any(diff(varDiffOrders)) == 0), ...
        'CHEBFUN:TREEVAR:sortConditions:multipleConditionsSameVariable', [...
        'Multiple initial conditions on the same variable/derivative '...
        'specified.'])

    % Check that values for all derivatives we expect to appear actually do so.
    % For IVPs/FVPs, the user has to specify the values of the function and all
    % appropriate derivatives.
    assert( all(diff(varDiffOrders) == 1) && ...
        length(varDiffOrders) == maxDiffOrders(varCounter), ...
        'CHEBFUN:TREEVAR:sortConditions:missingConditions', [...
        'Solving an nth order IVP/FVP requires specifying values of the \n' ...
        'solution and all the (n-1)st derivatives at the endpoint.'])
    assert( all( diff(varDiffOrders) == 1) && varDiffOrders(1) == 0, ...
        'myError');
    tempIndex = varList{varCounter}(diffOrderIndex);
    idx = [ idx, tempIndex ];
end

end

function accepted = acceptedCondition(tree)
%ACCEPTEDCONDITION   Check whether we have encountered an unsupported IC
%
% ACCEPTEDCONDITION(TREE) returns FALSE if we are working with functions such as
%   @(u) 5*u - 1
%   @(u) u + diff(u),
% which are not supported by the automatic first order reformulation. Otherwise,
% it returns TRUE.

% If TREE is not a struct, or it has height 0, it can't cause any harm:
if ( ~isstruct(tree) || ( tree.height == 0 ) )
    accepted = true;

% Deal with the case where the operator is + or -
elseif ( any(strcmp(tree.method, {'plus', 'minus'})) )

    % If TREE is of height 1, and it's operator is + or -, we're OK
    if ( tree.height == 1 )
        accepted = true;
        return
    end

    % Otherwise, we have to be more careful. If either argument is not a struct,
    % we only need to check the other. If both arguments are trees, we need
    % ensure that both don't have any IDs, and in the case that happens, check
    % the validity of each subtree.
    if ( ~isstruct(tree.left) )
        accepted = acceptedCondition(tree.right);
    elseif ( ~isstruct(tree.right) )
        accepted = acceptedCondition(tree.left);
    elseif ( any(tree.left.ID) && any(tree.right.ID) )
        accepted = false;
    else
        accepted = acceptedCondition(tree.right) && ...
            acceptedCondition(tree.right);
    end

% We're happy to support a tree with the diff method if it's height is 1:
elseif ( strcmp(tree.method, 'diff') && ( tree.height == 1 ) )
    accepted = true;

% Encountering any other operator implies that we had a condition on the form
% @(u) 5*u-1, which is not supported.
else
    accepted = false;
end

end


%  Infrared small target detection via line-based reconstruction and entropy-induced suppression
clearvars;
close all;
clc;
flag = 1;
wflag = 1;
channel = 1;
readimg;
for kk = 1
    fold = '.\data\';% 27 images
    try
        img = imread([fold, num2str(kk), '.jpg']);
    catch
        img = imread([fold, num2str(kk), '.bmp']);
    end
    img = img(:,:,1);
    img = double(img);
    img = ones(500, 500);
    img(:, 1:250) = 100;
    img(:, 251:end) = 180;
    img(100:104, 100:104) = 100 +100* fspecial('gaussian', [5, 5]);
    img = img(:,:,1);
    [ re, re1, re2] = lrfunc(img);
    bw = bwfunc(re);
    if flag
        figure; imshow(uint8(img));
        figure; mesh(img);
        figure; imshow(re, []);
        figure; imshow(re1, []);
        figure; imshow(re2, []);
    end
    if wflag
        refold = '.\'
        if ~exist(refold, 'dir')
            mkdir(refold);
        end
        imwrite(uint8(img), [refold, num2str(kk), '1.png']);
        imwrite(uint8( mat2gray(re) * 255), [refold, num2str(kk), '2.png']);
        imwrite(bw, [refold, num2str(kk), '3.png']);
    end
end


function LU = extract_bounds_from_norm_operator(LU,extstruct,extvariables,i);
arg = extstruct(i).arg{1};
if min(size(arg))==1 & length(extstruct(i).arg)>1
    p = extstruct(i).arg{2};
    % Maximize each element conservatively, and use the norm of
    % that worst-case vector
    xmax = abs(getbase(arg))*[1;max(abs(LU(getvariables(arg),:)),[],2)];
    LU(extvariables(i),2) = min([norm(xmax,p) LU(extvariables(i),2)]);
end
% norms are positive...
LU(extvariables(i),1) = max([0 LU(extvariables(i),1)]);


Network VGG16 {

	Layer CONV1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 64, C:3, R: 3, S: 3, Y: 224, X: 224 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV2 {
		Type: CONV
		Dimensions { K: 64, C: 64, R: 3, S: 3, Y: 224, X: 224 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}

	}

	Layer CONV3 {
		Type: CONV
		Dimensions { K: 128, C: 64, R: 3, S: 3, Y: 112, X: 112 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV4 {
		Type: CONV
		Dimensions { K: 128, C: 128, R: 3, S: 3, Y: 112, X: 112 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV5 {
		Type: CONV
		Dimensions { K: 256, C:128, R: 3, S: 3, Y: 56, X: 56 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV6 {
		Type: CONV
		Dimensions { K: 256, C: 256, Y: 56, X: 56, R: 3, S: 3 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV7 {
		Type: CONV
		Dimensions { K: 256, C: 256, R: 3, S: 3, Y: 56, X: 56 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV8 {
		Type: CONV
		Dimensions { K: 512, C: 256, R: 3, S: 3, Y: 28, X: 28 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV9 {
		Type: CONV
		Dimensions { K: 512, C:512, R: 3, S: 3, Y: 28, X: 28 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV10 {
		Type: CONV
		Dimensions { K: 512, C: 512, R: 3, S: 3, Y: 28, X: 28 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV11 {
		Type: CONV
		Dimensions { K: 512, C: 512, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV12 {
		Type: CONV
		Dimensions { K: 512, C: 512, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}

	Layer CONV13 {
		Type: CONV
		Dimensions { K: 512, C: 512, Y: 14, X: 14, R: 3, S: 3 }
		Dataflow {
			// Only one spatial map can be applied except on R and S
			TemporalMap(1,1) C;      // Flexible (Mapping size/offset, spatial/temporal)
			SpatialMap(1,1) K;       // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) Y';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(1,1) X';     // Flexible (Mapping size/offset, spatial/temporal)
			TemporalMap(Sz(R),Sz(R)) R; // This cannot be changed
			TemporalMap(Sz(S),Sz(S)) S; // This cannot be changed
			// Virtual Neuron of size Sz(R) x Sz(S):
			Cluster(Sz(R),P);
			SpatialMap(1,1) Y;
			SpatialMap(1,1) R;
			Cluster(Sz(S),P);
			SpatialMap(1,1) X;
			SpatialMap(1,1) S;
		}
	}
}


function[pixAcc, meanClassPixAcc] = evaluatePixAccHierarchy_preload(imageList, probs, ~, overlapListCell, superPixelLabelHistosCell, varargin)
% [pixAcc, meanClassPixAcc] = evaluatePixAccHierarchy_preload(imageList, probs, ~, varargin)
%
% Same as evaluatePixAccPaint, but much faster by taking into account the SS hierarchy.
% To use this hierarchy, run reconstructSelSearchHierarchyFromFz().
%
% Copyright by Holger Caesar, 2015

% Parse input
p = inputParser;
addParameter(p, 'printStatus', true);
parse(p, varargin{:});

printStatus = p.Results.printStatus;

% Init
labelCount = size(probs{1}, 2);
assert(labelCount > 1);
pixCorrectHisto = zeros(labelCount, 1);
pixTotalHisto = zeros(labelCount, 1);

imageCount = numel(imageList);
for imageIdx = 1 : imageCount,
    if printStatus,
        printProgress('Evaluating pixel accuracy for image', imageIdx, imageCount);
    end;

    % Skip images without ground-truth
    if isempty(probs{imageIdx}),
        continue;
    end;

    % Precompute maximum over labels
    [maxProbs, maxInds] = max(probs{imageIdx}, [], 2);

    % Compute maximum over regions (that contain a superpixel) and count pixels
    [pixCorrectHisto, pixTotalHisto] = evaluatePixAccHierarchy_loop(maxProbs, maxInds, full(overlapListCell{imageIdx}), superPixelLabelHistosCell{imageIdx}, pixCorrectHisto, pixTotalHisto);
end;

% Compute overall accuracies
pixAcc = sum(pixCorrectHisto) / sum(pixTotalHisto);
classPixAcc = pixCorrectHisto ./ pixTotalHisto;
meanClassPixAcc = nanmean(classPixAcc);


function img = atlasMapRgn2FullImage(img,imgRegion,X,Y,fillvalue)
%
%   img = atlasMapRgn2FullImage(img,imgRegion,X,Y,[fillvalue = -1])
%
% Author: Wandell
% Purpose:
%    We use atlasCreate (via CP2TFORM and IMTRANFORM) to transform the standard
%    atlas into the coordinate frame of the data. This produces a set of
%    standard atlas values that fall within an imgRegion inside the full
%    image of the data (img). The coordinates of imgRegion are at the
%    locations between X(1):X(2) and Y(1):Y(2) in the destination image,
%    img.  Because of the limitations on the affine transformations,
%    sometimes the data in imgRegion are   invalid,  having a fill value
%    (default of -1).  This routine copies the valid data in imgRegion into
%    the full image.
%
% Examples:
%    atlasE = atlasMapRgn2FullImage(atlasE,atlasET,X,Y);
%

if ~exist('fillvalue','var') , fillvalue = -1; end

xCoords = round(X(1):X(2));
yCoords = round(Y(1):Y(2));
for ii=1:length(xCoords)
    for jj=1:length(yCoords)
        if imgRegion(jj,ii) ~= fillvalue
            img(yCoords(jj),xCoords(ii)) = imgRegion(jj,ii);
        end
    end
end

return;


%% This function will determine the noise level
% Code By: K
% Last Updated: 2019/07/11
%%
function noise_level=Determine_Noise_Level(percent_iter)
% Set the noise level
if percent_iter==1
    noise_level=0;
elseif percent_iter==2
    noise_level=1e-7;
elseif percent_iter==3
    noise_level=5e-7;
elseif percent_iter==4
    noise_level=1e-6;
elseif percent_iter==5
    noise_level=5e-6;
elseif percent_iter==6
    noise_level=1e-5;
elseif percent_iter==7
    noise_level=5e-5;
elseif percent_iter==8
    noise_level=1e-4;
elseif percent_iter==9
    noise_level=5e-4;
elseif percent_iter==10
    noise_level=1e-3;
elseif percent_iter<=14
    noise_level=2e-3*(percent_iter-10);
elseif percent_iter==15
    noise_level=1e-2;
elseif percent_iter<=19
    noise_level=2e-2*(percent_iter-15);
elseif percent_iter==20
    noise_level=1e-1;
elseif percent_iter<=24
    noise_level=1e-1+1e-1*(percent_iter-20);
end


function [ids, dis]= yael_nn(v, q, k, distype)
    assert( nargin<4 || distype==2 );
    if nargin<3, k= 1; end
    assert(k<=size(v,2));

    ids= zeros(k, size(q,2), 'int32');
    dis= zeros(k, size(q,2), 'single');

    for iVec= 1:size(q,2)
        ds= sum( bsxfun(@minus, v, q(:,iVec)).^2, 1 );
        [ds, inds]= sort(ds);
        dis(:,iVec)= ds(1:k);
        ids(:,iVec)= inds(1:k);
    end
end


filename='Throne_Tetrahedra_SYM';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1];
constraint = {'volume'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'INCREASING LAST STEP';

nsteps = 10;
Vfrac_final = 0.4;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

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


%CMDL_HAT Create a CollisionModel object of the human HAT
%
% Returns a CollisionModel object of the human head and torso segment
% (also includes neck). The model may be given for any shoulder frame
% transformation (synonymous with the base transform of a HAL object).
% The head is a sphere, the neck a cylinder, the shoulders an ellipsoid
% and the torso an elliptical cylinder.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% Syntax:
%  (1) hat = cmdl_hat(Tg)
%  (2) hat = cmdl_hat()
%
%  (2) is as per (1), using HAL's default value of Tg
%
% Outputs:
%  hat : CollisionModel object of the human HAT
%
% Inputs:
%  Tg : Transformation frame of the shoulder
%
% See also anthroData cmdl_arm HAL

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.

function hat = cmdl_hat(Tg)

if ~nargin, Tg = trotx(pi/2); end

skin = pHRIWARE('skin');
shirt = pHRIWARE('orange');

% Anthropometric Data
[refnote, pb, sb, pn, sn, pg, sg, pt, st] = ...
    anthroData('pb', 'rb', 'pn', 'sn', 'pg', 'sg', 'pt', 'st');

% Head
gTb = [eye(3), pb; 0, 0, 0, 1];
Tb = Tg * gTb;
Head = Sphere(Tb, sb, 'FaceColor', skin, 'EdgeColor', 'none');

% Neck
gTn = [rotx(-pi/2), pn; 0, 0, 0, 1];
Tn = Tg * gTn;
Neck = Cylinder(Tn, sn, 'FaceColor', skin, 'EdgeColor', 'none');

% Shoulders
gT = [eye(3), pg; 0, 0, 0, 1];
T = Tg * gT;
Shoulders = Ellipsoid(T, sg, 'FaceColor', shirt, 'EdgeColor', 'none');

% Torso
gTt = [rotx(pi/2), pt; 0, 0, 0, 1];
Tt = Tg * gTt;
Torso = Cylinder(Tt, st, 'FaceColor', shirt, 'EdgeColor', 'none');

hat = CollisionModel(refnote, Head, Neck, Shoulders, Torso);

end


function r8_uniform_01_test ( )

%*****************************************************************************80
%
%% R8_UNIFORM_01_TEST tests R8_UNIFORM_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_UNIFORM_01_TEST\n' );
  fprintf ( 1, '  R8_UNIFORM_01 produces a sequence of random values.\n' );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  SEED = %d\n', seed );
  fprintf ( 1, '\n' );

  for i = 1 : 10
    [ r, seed ] = r8_uniform_01 ( seed );
    fprintf ( 1, '  %2d  %14f\n', i, r );
  end

  return
end


% The COBRAToolbox: testFASTCC.m
%
% Purpose:
%     - test FASTCC algorithm
%
% Authors:
%     - Original file: Thomas Pfau, May 2016
%     - Fix by @fplanes July 2017
%     - CI integration: Laurent Heirendt July 2017
%

% save the current path
currentDir = pwd;

% initialize the test
fileDir = fileparts(which('testFASTCC'));
cd(fileDir);

%load a model
model = getDistributedModel('ecoli_core_model.mat');
% create a model with fructose
modelWithFru = changeRxnBounds(model,'EX_fru(e)',-100,'l');

% set paarmeters
epsilon = 1e-4;
printLevel = 2;
modeFlag = 0;

%by default, fructose updatek, fumarate uptake and corresponding reactions
%cannot be used.
inactives = {'EX_fru(e)', 'EX_fum(e)','EX_gln_L(e)', 'EX_mal_L(e)', 'FRUpts2', 'FUMt2_2', 'GLNabc','MALt2_2' };
fructoseRelated = {'EX_fru(e)', 'FRUpts2'};

% define the solver packages to be used to run this test
solverPkgs = prepareTest('needsLP',true);

for k = 1:length(solverPkgs.LP)
    % change the COBRA solver (LP)
    solverOK = changeCobraSolver(solverPkgs.LP{k}, 'LP', 0);
    fprintf('   Testing FASTCC using %s ... \n', solverPkgs.LP{k});
    A = fastcc(model, epsilon, printLevel,modeFlag);
    assert(isempty(setxor(setdiff(model.rxns,model.rxns(A)),inactives)));
    % Open up the Fructose channel
    A = fastcc(modelWithFru, epsilon, printLevel,modeFlag);
    % now, everything from the inactives except the fructose reactions are
    % not in A
    assert(isempty(setxor(setdiff(modelWithFru.rxns,modelWithFru.rxns(A)),setdiff(inactives,fructoseRelated))));
    % output a success message
    fprintf('Done.\n');
end

% change the directory
cd(currentDir)


function [candidates, scores] = get_candidates(candidate_dir,method_config, img_id, num_candidates, allow_filtering)

  if nargin < 4
    allow_filtering = true;
  end

  [candidates, scores, rerun_num_candidates] = read_candidates_mat(candidate_dir, img_id);
  if iscell(candidates) && iscell(scores)
    [~,idx] = min(abs(rerun_num_candidates - num_candidates));
    candidates = candidates{idx};
    scores = scores{idx};
  end

  if allow_filtering
    if strcmp(method_config.opts.order, 'none')
      % nothing to do
    elseif strcmp(method_config.opts.order, 'biggest')
      w = candidates(:,3) - candidates(:,1) + 1;
      h = candidates(:,4) - candidates(:,2) + 1;
      areas = w .* h;
      [~,order] = sort(areas, 'descend');
      candidates = candidates(order,:);
      scores = scores(order,:);
    elseif strcmp(method_config.opts.order, 'smallest')
      w = candidates(:,3) - candidates(:,1) + 1;
      h = candidates(:,4) - candidates(:,2) + 1;
      areas = w .* h;
      [~,order] = sort(areas, 'ascend');
      candidates = candidates(order,:);
      scores = scores(order,:);
    elseif strcmp(method_config.opts.order, 'random')
      s = RandStream('mt19937ar','Seed',0);
      perm = randperm(s, size(candidates,1));
      candidates = candidates(perm,:);
      if numel(scores) > 0
        scores = scores(perm);
      end
    else
      [scores, argsort] = sort(scores, method_config.opts.order);
      candidates = candidates(argsort,:);
    end

    num_candidates = min(num_candidates, size(candidates, 1));
    candidates = candidates(1:num_candidates,:);
    if numel(scores) > 0
      scores = scores(1:num_candidates,:);
    end
  else
    error('this shouldn''t be used');
  end
end


% Fig. 5.19   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%
clf
n=1;
d=conv([1 2 0],[1 2 5]);
pzmap(n,d)
axis([-4 4 -3 3])
title('Fig.5.19 Departure angle calculation')
z=0:.1:.9;
 wn=1:1:4;
 sgrid(z, wn)
 hold off


function varargout = plotEarth(varargin)
%PLOTEARTH  Plots the landmasses of earth.
%   PLOTEARTH plots outlines of the landmasses of earth on a sphere using
%   black solid lines.
%
%   PLOTEARTH(LINESPEC) uses the uses the color and marker from the
%   line specification string 'LineSpec' (See PLOT for possibilities)
%
% See also spherefun/surf, spherefun/plot, spherefun/contour

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

% Land masses are stored in the data file CoastData.mat
try
    x = load('CoastData.mat', 'coast');
catch
    error('CHEBFUN:SPHEREFUN:PLOTEARTH:coastDataNotFound',...
        ['File containing the coast line data could not be found. '...
        'Try reinstalling chebfun.']);
end

if ( nargin > 0 )
    linespec = varargin{:};
else
    linespec = 'k-';
end

% Get the hold state of the current axis:
holdState = ishold;

if ( ~holdState )
    % Generate a unit sphere.
    [XX,YY,ZZ] = sphere(101);

    % Color of the sphere will be white:
    clr = [255 255 255]/255;

    % Plot the sphere, make it slightly smaller than unit so lines
    % show up more clearly.
    scl = 0.99;

    surf(scl*XX, scl*YY, scl*ZZ, 1+0*XX, 'EdgeColor', 'None', ...
        'FaceColor', clr);
    daspect([1 1 1]);
    hold on
end

h = plot3(x.coast(:,1), x.coast(:,2), x.coast(:,3), linespec);

if ( ~holdState )
    hold off
end

if ( nargout > 0 )
    varargout = { h };
end

end


%% OPT_POOL uses the MATLABPOOL command to run the QUAD code.
%
%  Discussion:
%
%    Output printed by the function appears directly on the screen.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2010
%
%  Author:
%
%    John Burkardt
%
  n = 16;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'OPT_POOL\n' );
  fprintf ( 1, '  Solve optimization problem with discretization parameter N = %d\n', n );

  matlabpool open local 4

  [ z_star, PAR, exit ] = opt_fun ( @zermelo, n );

  matlabpool close
%
%  On successful conclusion, display the plot.
%
  if ( 0 <= exit && isfield ( PAR, 'plot' ) )
    feval ( PAR.plot, z_star, PAR )
  end


function ChannelMat = in_channel_curry_rs3(ChannelFile)
% IN_CHANNEL_CURRY_RS3:  Read 3D cartesian positions for a set of electrodes from Curry .rs3 file.
%
% USAGE:  ChannelMat = in_channel_curry_res3(ChannelFile)
%
% INPUTS:
%     - ChannelFile : Full path to the file

% @=============================================================================
% 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.
% =============================================================================@
%
% Authors: Francois Tadel, 2009-2011

% Open file
fid = fopen(ChannelFile, 'r');
if (fid == -1)
    error('Cannot open file.');
end
% Initialize indices structure
iChannel = 1;
curBlock = '';
ChannelMat = db_template('channelmat');
ChannelMat.Comment = 'Curry rs3';

% Read file line by line
while 1
    % Read line
    read_line = fgetl(fid);
    % Empty line: go to next line
    if isempty(read_line)
        continue
    end
    % End of file: stop reading
    if (read_line(1) == -1)
        break
    end

    % Check if beginning/end of block

    if ~isempty(regexp(read_line, 'SENSORS\w* START_LIST'))
        curBlock = 'pos';
        iChannel = 1;
    elseif ~isempty(regexp(read_line, 'LABELS\w* START_LIST'))
        curBlock = 'labels';
        iChannel = 1;
    elseif ~isempty(regexp(read_line, 'SENSORS\w* END_LIST')) || ~isempty(regexp(read_line, 'LABELS\w* END_LIST'))
        curBlock = '';
    else
        switch (curBlock)
            case 'labels'
                ChannelMat.Channel(iChannel).Name = read_line;
                iChannel = iChannel + 1;
            case 'pos'
                xyz = sscanf(read_line, '%f %f %f') / 1000;
                ChannelMat.Channel(iChannel).Type    = 'EEG';
                ChannelMat.Channel(iChannel).Loc     = [-xyz(2); xyz(1); xyz(3)];
                ChannelMat.Channel(iChannel).Orient  = [];
                ChannelMat.Channel(iChannel).Comment = '';
                ChannelMat.Channel(iChannel).Weight  = 1;
                iChannel = iChannel + 1;
        end
    end
end
% Close file
fclose(fid);


%% Creating one CUDA accelerated test radiography

%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Stefanie Kaser
%--------------------------------------------------------------------------
%% pCT geometry definition

geo.dDetector = [0.25; 0.25]; % Pixel size of optimized pRad in mm.
geo.DSID = 245; % Distance between source and upstream detector.
geo.DSO = 300; % Distance between source and origin (center of rotation).
geo.DSD = 355; % Distance between source and downstream detector.
geo.hull = [0; 0; 0; 0]; % We won't use a convex hull here (all entries are
% set to 0).
geo.sDetector = [20; 20]; % Defines the size (in mm) of the optimized pRad.
geo.mode = 'parallel'; % or 'cone' in case of cone beam geometry

%% pCT initial beam energy
eIn = single(100);

%% Load pCT data set.
% The data set corresponds to one radiographic image (pRad) of an Aluminum
% stair profile as used in https://arxiv.org/abs/2106.12890. However, our
% test data set is based on Monte Carlo simulations (GATE:
% doi.org/10.1016/S0920-5632(03)90969-8). The data set contains the
% protons' upstream and downstream positions and directions as well as the
% single proton WEPLs (all in mm).
data = pCTdata();

%% Binning the data into an optimized pRad
% Finally, the single proton data are binned into an optimized pRad.
proj = pCTCubicSpline_mex(data.posIn, data.posOut, data.dirIn, ...
    data.dirOut, data.Wepl, eIn, geo);
% We are only creating one test pRad here. Creating pRads at multiple
% rotation angles would allow to use the collection as input for TIGRE's
% reconstruction algorithms (see d04_SimpleReconstruction.m).

%% Plot the result
imshow(proj, [0, 3], 'InitialMagnification', 500);


function matched_points=it_features_matching(upl_1, lor_1, upl_2, lor_2, th, cal, matches, past_weigth)

%% ITERATIVE FEATURES MATCHING
%% As features are selected the best matching in any iteration.
%% Then it is recursively refined till reaching the threshold size
%% subwindows

%% Input
%% iml,imr       : templates to be matched
%% upl_1, upl_2  : image coordinates of the templates' upper left corner
%% lor_1, lor_2  : image coordinates of the templates' lower right corner
%% th            : matching threshold
%% cal           : (1=rectified images with epipolar horizontal lines; 0=uncalibrated images )
%% matches       : N x 4 matrix comprised of the already matched points
%% past weigth   : present match weigth

%% Output
%% matched_points : Correspondences matrix (N x 4)
%%                  [v1_1 u1_1 v2_1 u2_1
%%                   v1_2 u1_2 v2_2 u2_2
%%                  ...
%%                   v1_N u1_N v1_N u1_N]

global IM1 IM2 IM1_X IM1_Y IM1_X2 IM1_Y2 SIGMA_MAX;
offset=0;
imw1=IM1(upl_1(1):lor_1(1),upl_1(2):lor_1(2));
imw2=IM2(upl_2(1):lor_2(1),upl_2(2):lor_2(2));

[mt1,mt2,upl_mt1,lor_mt1,upl_mt2,lor_mt2,pres_weigth,err]=temp_temp_matching(imw1,imw2,upl_1,lor_1,upl_2,lor_2,th,cal);
pres_weigth=pres_weigth+past_weigth;
[tysize,txsize]=size(mt1);

if (~err)  %% returned valid match

    if (tysize<8 | txsize<8)  %% last step. The central position of the matched subwindows are added to the correspondences set.
        matched_points=[matches; (upl_mt1+lor_mt1)/2, (upl_mt2+lor_mt2)/2, pres_weigth];

    else
        %% matched subwindows tiling
        y1=floor(tysize/2)-offset;
        y2=floor(tysize/2)+offset;
        x1=floor(txsize/2)-offset;
        x2=floor(txsize/2)+offset;

        %hi-le templates
        t1_1=mt1( 1:y2 , 1:x2 );
        upl_1_1=upl_mt1 + [0,0];
        lor_1_1=upl_mt1 + [y2-1,x2-1];

        t2_1=mt2( 1:y2  , 1:x2 );
        upl_2_1=upl_mt2 + [0,0];
        lor_2_1=upl_mt2 + [y2-1,x2-1];

        %hi-ri templates
        t1_2=mt1( 1:y2 , x1:txsize );
        upl_1_2=upl_mt1 + [0,x1-1];
        lor_1_2=upl_mt1 + [y2-1,txsize-1];

        t2_2=mt2( 1:y2 , x1:txsize );
        upl_2_2=upl_mt2 + [0,x1-1];
        lor_2_2=upl_mt2 + [y2-1,txsize-1];

        %lo-le templates
        t1_3=mt1( y1:tysize , 1:x2 );
        upl_1_3=upl_mt1 + [y1-1,0];
        lor_1_3=upl_mt1 + [tysize-1,x2-1];

        t2_3=mt2( y1:tysize , 1:x2 );
        upl_2_3=upl_mt2 + [y1-1,0];
        lor_2_3=upl_mt2 + [tysize-1,x2-1];

        %lo-ri templates
        t1_4=mt1( y1:tysize , x1:txsize );
        upl_1_4=upl_mt1 + [y1-1,x1-1];
        lor_1_4=upl_mt1 + [tysize-1,txsize-1];

        t2_4=mt2( y1:tysize , x1:txsize );
        upl_2_4=upl_mt2 + [y1-1,x1-1];
        lor_2_4=upl_mt2 + [tysize-1,txsize-1];

        matched_points=[matches;
                             it_features_matching(upl_1_1,lor_1_1,upl_2_1,lor_2_1,th,cal,matches,pres_weigth);
                             it_features_matching(upl_1_2,lor_1_2,upl_2_2,lor_2_2,th,cal,matches,pres_weigth);
                             it_features_matching(upl_1_3,lor_1_3,upl_2_3,lor_2_3,th,cal,matches,pres_weigth);
                             it_features_matching(upl_1_4,lor_1_4,upl_2_4,lor_2_4,th,cal,matches,pres_weigth)];

     end

 else
     matched_points=matches;
 end


function c = butter(c,varargin)

% C = BUTTER(C,[TYPE],CUTOFF,[POLES])
% This function creates and applies a butterworth filter to each waveform
% in the correlation object. The function returns a correlation object.
% Filtering is performed by an underlying call to the filterobject/filtfilt
% routine. Note that filtfilt applies the filter once in each direction to
% minimize phase distortion (effectively a zero-phase filter). This also
% has the effect of doubling the order of the filter. 2 or 4 poles should
% be sufficient for most applications.
%
% EXAMPLES:
%  c = BUTTER(c,[1 5])            band pass filter on 1-5 Hz (4 poles)
%  c = BUTTER(c,'B',[1 5])        same as previous example
%  c = BUTTER(c,'L',5)            low  pass filter below 5 Hz (4 poles)
%  c = BUTTER(c,'H',1)            high pass filter above 5 Hz (4 poles)
%  c = BUTTER(c,...,2)            use 2 poles

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$
% TODO: should check to ensure low cutoff period is >> trace length

% GET INPUTS
if ischar(varargin{1})       % set filter type
    type = varargin{1};
    varargin = varargin(2:end);
else
    type = 'B';
end

cutoff = varargin{1};            % set cutoff frequencies

if length(varargin)>1
   poles = varargin{2};
else
    poles = 4;                  % set number of poles
end

% CHECK FIRST LETTER OF FILTER TYPE
type = upper(type(1));
if (type~='B') && (type~='H') && (type~='L')
    error('Filter type not recognized')
end;

% CHECK NUMBER OF CUTOFF FREQUENCIES
if type=='B'
    if length(cutoff)~=2
        error('Two cutoff frequecies needed for bandpass filter')
    end;
else
    if length(cutoff)~=1
        error('High and lowpass filters require one cutoff frequency')
    end;
end;

% CHECK FREQUENCY RANGE
if cutoff(end) >= get(c,'NYQ')
    error('Frequency cutoffs exceed the Nyquist frequency')
end;

% duration = get(c,'DURATION_EPOCH');
% if 1/cutoff(1) >= duration(1)
%     warning('Frequency cutoff is very low relative to trace length');
% end;
% uration(1)
% 1/cutoff(1)

% APPLY FILTER
f = filterobject(type,cutoff,poles);
c.W = filtfilt(f,c.W);


function inum=myRound(dnum)

if dnum>0
    inum=fix(dnum+0.5);
else
    inum=fix(dnum-0.5);
end

return


function [xs,optimState] = searchCMA(sumrule,x,gpstruct,LB,UB,optimState,options)
%SEARCHCMA Covariance matrix adaptation search step (inspired by CMA-ES).

if nargin < 2 || isempty(x) || isempty(gpstruct)
    xs = 'cma';
    return;
end

% SUMRULE not specified, all arguments shifted by one
if nargin < 7
    options = optimState;
    optimState = UB;
    UB = LB;
    LB = gpstruct;
    gpstruct = x;
    x = sumrule;
    sumrule = [];
end

% By default renormalize by expected vector magnitude
if isempty(sumrule); sumrule = 1; end

MeshSize = optimState.meshsize;
SearchFactor = optimState.searchfactor;

nvars = length(x);

sigma = optimState.C';
sigma = MeshSize*SearchFactor*sigma;    % Rescale by current scale

N = options.Nsearch;
xs = bsxfun(@plus, x, randn(N,nvars)*sigma);

end


function test_suite = test_linePosition
%TESTLINEPOSITION  One-line description here, please.
%
%   output = testLinePosition(input)
%
%   Example
%   testLinePosition
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-06-15,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

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

point = [20 60];
line = createLine([10 30], [30 90]);

exp = .5;
pos = linePosition(point, line);

testCase.assertEqual(exp, pos);

function testPointArray(testCase)

point = [20 60;10 30;25 75];
line = createLine([10 30], [30 90]);

exp = [.5; 0; .75];
pos = linePosition(point, line);

testCase.assertEqual(exp, pos);

function testLineArray(testCase)

point = [20 60];
line1 = createLine([10 30], [30 90]);
line2 = createLine([0 0], [20 60]);
line3 = createLine([20 60], [40 120]);
lines = [line1;line2;line3];

exp = [.5  1  0];
pos = linePosition(point, lines);

testCase.assertEqual(exp, pos);

function testArrayArray(testCase)

lines = [10 20 1 0;10 20 0 1;10 20 1 1];
points = [30 20;10 40; 30 40];

exp = [20 0 10; 0 20 10; 20 20 20];
pos = linePosition(points, lines);

testCase.assertEqual(exp, pos, 'AbsTol', .01);

function testArrayArrayDiag(testCase)

lines = [10 20 1 0;10 20 0 1;10 20 1 1];
points = [30 20;10 40; 30 40];

exp = [20; 20; 20];
pos = linePosition(points, lines, 'diag');

testCase.assertEqual(exp, pos, 'AbsTol', .01);


function response = Forgetful(n,p)

persistent ncount trustcount roundstore
if n==1; ncount=0; trustcount=0; roundstore=0; end
if ncount==1 && (p==1 || p==5); trustcount=trustcount+1; end

if n>5
    if isequal( roundstore(end-3:end), [5 0 5 0] );
        trustcount = 2;
        ncount = 5;
    end
end

if ( p==1 || p==5 || n==20 ) && ncount<5 && n~=1
    response = 'defect';
else
    response = 'cooperate';
    if trustcount>2; response = 'defect'; end
    ncount = 0;
end

roundstore(n) = p;

ncount = ncount + 1;


load('ROIs/CC')

h = guidata(1);
coords = h.rois(h.curRoi).coords;
% Re-index starting at 1
coords = coords-repmat(min(coords),size(coords,1),1)+1;
coords = round(coords);
voxels = zeros(max(coords));
ind = sub2ind(size(voxels), coords(:,1), coords(:,2), coords(:,3));
voxels(ind) = 1;
voxels = smooth3(voxels,'gaussian',5);
voxels = voxels>.2;

voxels = dtiCleanImageMask(voxels, 5);
mm = [1 1 1];
id = -1;
[id,wasOpen] = mrmCheckMeshServer(id, 'localhost');
[mesh,lights,tenseMesh] = mrmBuildMesh(uint8(voxels), mm, 'localhost', id);


function G = grad3(V,T)
  % GRAD3 Compute the numerical gradient operator for tet meshes in 3d
  %
  % G = grad(V,F)
  %
  % Inputs:
  %   V  #vertices by dim list of mesh vertex positions
  %   T  #elements by 3 list of mesh tet indices
  % Outputs:
  %   G  #elements*dim by #V Gradient operator
  %
  % Example:
  %   L = cotmatrix(V,T);
  %   G  = grad3(V,T);
  %   vol = volume(V,T);
  %   GMG = -G'*repdiag(diag(sparse(vol)),3)*G;
  %

  warning('Deprecated. Call grad directly.');
  G = grad(V,T);
end


% Recording Particle Velocity Example
%
% This example demonstrates how to record the particle velocity using a
% Cartesian or binary sensor mask. It builds on the Homogeneous Propagation
% Medium and Heterogeneous Propagation Medium examples.
%
% author: Bradley Treeby
% date: 1st November 2010
% last update: 20th September 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave 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 Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

clear all;

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 128;           % number of grid points in the x (row) direction
Ny = 128;           % number of grid points in the y (column) direction
dx = 0.1e-3;        % grid point spacing in the x direction [m]
dy = 0.1e-3;        % grid point spacing in the y direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy);

% define the properties of the propagation medium
medium.sound_speed = 1500;  % [m/s]
medium.density = 1000;      % [kg/m^3]
medium.alpha_coeff = 0.75;  % [dB/(MHz^y cm)]
medium.alpha_power = 1.5;

% create time array
t_end = 6e-6;
kgrid.t_array = makeTime(kgrid, medium.sound_speed, [], t_end);

% create initial pressure distribution using makeDisc
disc_magnitude = 5; % [au]
disc_x_pos = Nx/2;  % [grid points]
disc_y_pos = Ny/2;  % [grid points]
disc_radius = 5;    % [grid points]
source.p0 = disc_magnitude*makeDisc(Nx, Ny, disc_x_pos, disc_y_pos, disc_radius);

% define four sensor points centered about source.p0
sensor_radius = 40; % [grid points]
sensor.mask = zeros(Nx, Ny);
sensor.mask(Nx/2 + sensor_radius, Ny/2) = 1;
sensor.mask(Nx/2 - sensor_radius, Ny/2) = 1;
sensor.mask(Nx/2, Ny/2 + sensor_radius) = 1;
sensor.mask(Nx/2, Ny/2 - sensor_radius) = 1;

% set the acoustic variables that are recorded
sensor.record = {'p', 'u'};

% run the simulation
sensor_data = kspaceFirstOrder2D(kgrid, medium, source, sensor);

% =========================================================================
% VISUALISATION
% =========================================================================

% plot the initial pressure and sensor distribution
figure;
imagesc(kgrid.y_vec*1e3, kgrid.x_vec*1e3, source.p0 + sensor.mask, [-1 1]);
colormap(getColorMap);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;

% plot the simulated sensor data
figure;
[t, t_sc, t_prefix] = scaleSI(kgrid.t_array(end));
mx = 5e-7;
for sensor_num = 1:4
    % plot the pressure
    subplot(4, 3, 3*sensor_num - 2), plot(t_sc*kgrid.t_array, sensor_data.p(sensor_num, :), 'k-');
    set(gca, 'YLim', [-0.75, 0.75], 'XLim', [0 t_end*t_sc]);
    xlabel(['time [' t_prefix 's]']);
    ylabel('p');

    % plot the particle velocity ux
    subplot(4, 3, 3*sensor_num - 1), plot(t_sc*kgrid.t_array, sensor_data.ux(sensor_num, :), 'k-');
    set(gca, 'YLim', [-mx, mx], 'XLim', [0 t_end*t_sc]);
    xlabel(['time [' t_prefix 's]']);
    ylabel('ux');

    % plot the particle velocity uz
    subplot(4, 3, 3*sensor_num), plot(t_sc*kgrid.t_array, sensor_data.uy(sensor_num, :), 'k-');
    set(gca, 'YLim', [-mx, mx], 'XLim', [0 t_end*t_sc]);
    xlabel(['time [' t_prefix 's]']);
    ylabel('uy');
end


function [splitPsi, logQ] = sampleSplitConfig( Psi, data, anchorObjIDs, featIDs, algParams, TargetPsi )
% Sample a proposal config of (F,z) that has one extra state than input
%INPUT:
%  Psi : input model config (contains F,z)
%  data : data object
%  anchorObjIDs : length 2 vector giving ids of anchor objects for proposal
%  featIDs : integer id of feature in Psi to split into two
%  TargetPsi : target model config.
%    when provided, we don't actually sample, but instead calculate
%    probability "logQ" of sampling the target given the input Psi config
%OUTPUT:
%  splitPsi : output configuration (proposal)
%    has all its observed data sufficent statistics pre-calculated
%    for easy use in later calculations (accept ratios, etc.
%  logQ     : struct of transition probabilities (in log space)

% ------------------------------------------------------- INTERP INPUT
anchorObjIDs = anchorObjIDs(:)';
ii = anchorObjIDs(1);
jj = anchorObjIDs(2);

kold = featIDs(1);
otherObjIDs = setdiff( find( Psi.F( :, kold ) > 0   )', anchorObjIDs );
activeObjIDs = [anchorObjIDs otherObjIDs];

if exist( 'TargetPsi', 'var' ) && ~isempty( 'TargetPsi' )
   kA = TargetPsi.activeFeatIDs(1);
   kB = TargetPsi.activeFeatIDs(2);
else
    TargetPsi = [];
    kA = kold;
    kB = size(Psi.F,2) + 1;
end

% ------------------------------------------------------- INIT PROPOSAL
propF = Psi.F == 1;
propF(:, kold) = 0;
propF( ii, kA) = 1;
propF( jj, kB) = 1;
propFeatIDs = [kA kB];

propStateSeq = Psi.stateSeq;
for aa = 1:data.N
    propStateSeq( aa ).z( Psi.stateSeq(aa).z == kold ) = 0;
end
propStateSeq( ii ).z(  Psi.stateSeq( ii ).z == kold )  = kA;
propStateSeq( jj ).z(  Psi.stateSeq( jj ).z == kold )  = kB;

% Create deterministic trans params Eta
propTransM = Psi.TransM.getAllEta_PriorMean( activeObjIDs, propF, propFeatIDs );

% Create deterministic Theta for ALL features: others + [kA kB]
propThetaM = Psi.ThetaM.getAllTheta_PosteriorMean( data, propStateSeq, propFeatIDs, featIDs);
assert( propThetaM.K == length(propThetaM.Xstats), 'Badness');

% Init suff stats for F
featCounts = sum( propF(:,propFeatIDs) , 1 );
nObj = size(propF, 1) - length(otherObjIDs);
logQ.F = 0;
logQ.z = 0;

proposalsON = false(1, size(propF,2) );
proposalsON(propFeatIDs) = 1;
for aa = [ otherObjIDs( randperm(length(otherObjIDs)) ) ii jj]

    if aa == ii || aa == jj
        % Make sure to update F suff. stats for anchors
        %  since we've already "seen" them before, unlike other sequences
        featCounts = featCounts - propF(aa, propFeatIDs);
        nObj = nObj - 1;
        propThetaM = propThetaM.decXStats( aa, data, propStateSeq, propFeatIDs );
    end

    % ====================================================  Sample F(aa,:)
    % Calc Soft Evidence for the current sequence, including both new feats
    allActiveFeatIDs = find( propF(aa,:) | proposalsON );
    seqSoftEv = propThetaM.calcLogSoftEv( aa, data, allActiveFeatIDs );

    % Propose new feature assignments for the cur. seq.
    priorPrFeats = featCounts ./ ( nObj + Psi.bpM.c );
    [newFaa, logQ_Faa] = sampleTwoSharedFeatsForSeq_Gibbs( aa, propFeatIDs, seqSoftEv, propTransM.seq(aa), priorPrFeats, anchorObjIDs, TargetPsi);
    propF(aa, propFeatIDs) = newFaa==1;
    logQ.F = logQ.F + logQ_Faa;

    % ===============================================  Sample stateSeq(aa)
    seqTransM = propTransM.getSeqEtaWithFeats( aa, propF(aa,:) );
    propTransM = propTransM.setEta( aa, propF(aa,:), seqTransM.eta );
    [propStateSeq(aa).z, logQ_zaa] = sampleSingleStateSeq_WithSoftEv( aa, seqTransM, seqSoftEv(propF(aa,:), :), TargetPsi);
    logQ.z = logQ.z + logQ_zaa;

    % Update suff. stats for F
    featCounts = featCounts + propF(aa, propFeatIDs);
    nObj = nObj + 1;

    % Update Emission Params for new proposed features [kA, kB]
    %   using the newly allocated data from the current sequence
    propThetaM = propThetaM.incXStats( aa, data, propStateSeq, propFeatIDs );
    for kkID = 1:length(propFeatIDs)
        kk = propFeatIDs(kkID);
        PN = propThetaM.getPosteriorParams( propThetaM.Xstats(kk) );
        propThetaM.theta(kk) = propThetaM.getTheta_Mean( PN );
    end
end

if isempty( TargetPsi )
    propThetaM = propThetaM.updateAllXSuffStats( horzcat(propStateSeq(:).z), data );
end

splitPsi.F = propF;
splitPsi.stateSeq = propStateSeq;
splitPsi.ThetaM = propThetaM;
splitPsi.TransM = propTransM;
splitPsi.bpM = Psi.bpM;
splitPsi.activeFeatIDs = propFeatIDs;
splitPsi.anchorIDs  = anchorObjIDs;


function IntProj(ana, hs, proj)
%
%  A function to perform Minimum or Maximum intensity projection of a volume of data
%  ana: Name of AFNI brick file
%  hs: Number of slices to use is each direction. Projection at slice i is from i-hs to i+hs
%      hs stands for half slab
%  proj: (-1) minimum intensity projection
%          1  maximum intensity projection
%  For output, the function creates
% see also insane function Int_Proj

FuncName = 'IntProj';

if (proj == -1) proj_s = 'min';
elseif (proj == 1) proj_s = 'max';
else
   fprintf(2,'Error %s: Bad projection parameter\n', FuncName);
   return;
end

[ana_pref, ana_view] = AfniPrefix(ana);
ana = sprintf('%s%s', ana_pref, ana_view);

[err, V, V_Info] = BrikLoad(ana);
Nvox = V_Info.DATASET_DIMENSIONS;

verb = 1;

%padd V by half slab in all directions to fix edge effects
Vp = zeros(Nvox(1)+3.*hs, Nvox(2)+3.*hs, Nvox(3)+3.*hs);
Vp(hs+1:Nvox(1)+hs, hs+1:Nvox(2)+hs, hs+1:Nvox(3)+hs) = V; clear V;

for (pd = 1:1:3), % projection direction
   if (pd == 1) dc = 'I';
   elseif (pd == 2) dc = 'J';
   elseif (pd == 3) dc = 'K';
   else
      fprintf(2,'Error %s: Bad projection direction\n', FuncName);
      return;
   end

   if (verb) fprintf(1,'%s: Processing direction %s\n', FuncName, dc); end
   %create the output set
   Vmi = zeros(Nvox(1), Nvox(2), Nvox(3));

   if (proj == -1),
      if (pd == 1),
         for (i=hs+1:1:Nvox(1)+hs),
            Vmi(i-hs,:,:) = min(Vp(i-hs:i+hs,hs+1:Nvox(2)+hs,hs+1:Nvox(3)+hs), [], pd);
         end
         Vmall = Vmi;
      elseif (pd == 2),
         for (i=hs+1:1:Nvox(2)+hs),
            Vmi(:,i-hs,:) = min(Vp(hs+1:Nvox(1)+hs,i-hs:i+hs,hs+1:Nvox(3)+hs), [], pd);
         end
         Vmall(:) = min(Vmi(:),Vmall(:));
      elseif (pd == 3),
         for (i=hs+1:1:Nvox(3)+hs),
            Vmi(:,:,i-hs) = min(Vp(hs+1:Nvox(1)+hs,hs+1:Nvox(2)+hs,i-hs:i+hs), [], pd);
         end
         Vmall(:) = min(Vmi(:),Vmall(:));
      end
   elseif (proj == 1),
      if (pd == 1),
         for (i=hs+1:1:Nvox(1)+hs),
            Vmi(i-hs,:,:) = max(Vp(i-hs:i+hs,hs+1:Nvox(2)+hs,hs+1:Nvox(3)+hs), [], pd);
         end
         Vmall = Vmi;
      elseif (pd == 2),
         for (i=hs+1:1:Nvox(2)+hs),
            Vmi(:,i-hs,:) = max(Vp(hs+1:Nvox(1)+hs,i-hs:i+hs,hs+1:Nvox(3)+hs), [], pd);
         end
         Vmall(:) = max(Vmi(:),Vmall(:));
      elseif (pd == 3),
         for (i=hs+1:1:Nvox(3)+hs),
            Vmi(:,:,i-hs) = max(Vp(hs+1:Nvox(1)+hs,hs+1:Nvox(2)+hs,i-hs:i+hs), [], pd);
         end
         Vmall(:) = max(Vmi(:),Vmall(:));
      end

   end
   OptW.Prefix = sprintf('%s_%s%g%s', ana_pref, proj_s, hs, dc);
   OptW.Scale = 1;
   WriteBrik(Vmi, V_Info, OptW);
end

   OptW.Prefix = sprintf('%s_%s%gIJK', ana_pref, proj_s, hs);
   OptW.Scale = 1;
   WriteBrik(Vmall, V_Info, OptW);

return;