function [grid, cfg] = ft_prepare_sourcemodel(cfg, vol, sens)

% FT_PREPARE_SOURCEMODEL helps to make a source model that can be
% used for source reconstruction, beamformer scanning, linear
% estimation and MEG interpolation.
%
% Use as
%   [grid, cfg] = prepare_dipole_grid(cfg)
% where the configuration structure contains the details on how the source
% model should be constructed.
%
% A source model can be constructed based on
%   - regular 3D grid with explicit specification
%   - regular 3D grid with specification of the resolution
%   - regular 3D grid, based on segmented MRI, restricted to gray matter
%   - surface grid based on brain surface from volume conductor
%   - surface grid based on head surface from external file
%   - using user-supplied grid positions, which can be regular or irregular
%   - cortical sheet that was created in MNE or Freesurfer
% The approach that will be used depends on the configuration options that
% you specify.
%
% Configuration options for generating a regular 3-D grid
%   cfg.grid.xgrid      = vector (e.g. -20:1:20) or 'auto' (default = 'auto')
%   cfg.grid.ygrid      = vector (e.g. -20:1:20) or 'auto' (default = 'auto')
%   cfg.grid.zgrid      = vector (e.g.   0:1:20) or 'auto' (default = 'auto')
%   cfg.grid.resolution = number (e.g. 1 cm) for automatic grid generation
%
% Configuration options for a predefined grid
%   cfg.grid.pos        = N*3 matrix with position of each source
%   cfg.grid.dim        = [Nx Ny Nz] vector with dimensions in case of 3-D grid (optional)
%   cfg.grid.inside     = vector with indices of the sources inside the brain (optional)
%   cfg.grid.outside    = vector with indices of the sources outside the brain (optional)
% The following fields are not used in this function, but will be copied along to the output
%   cfg.grid.leadfield
%   cfg.grid.filter or alternatively cfg.grid.avg.filter
%   cfg.grid.subspace
%   cfg.grid.lbex
%
% Configuration options for cortex segmentation, i.e. for placing dipoles in grey matter
%   cfg.mri           = can be filename, MRI structure or segmented MRI structure
%   cfg.sourceunits   = 'mm' or 'cm' (default is 'cm')
%   cfg.mriunits      = 'mm' or 'cm' (default is 'mm')
%   cfg.threshold     = 0.1, relative to the maximum value in the segmentation
%   cfg.smooth        = 5, smoothing in voxels
%
% Configuration options for reading a cortical sheet from file
%   cfg.headshape     = string, should be a *.fif file
%
% Other configuration options
%   cfg.vol          = volume conduction model
%   cfg.grad         = gradiometer definition
%   cfg.elec         = electrode definition
%   cfg.grid.tight   = 'yes' or 'no' (default is automatic)
%   cfg.inwardshift  = depth of the bounding layer for the source space, relative to the head model surface (default = 0)
%   cfg.symmetry     = 'x', 'y' or 'z' symmetry for two dipoles, can be empty (default = [])
%   cfg.headshape    = a filename containing headshape, a structure containing a
%                      single triangulated boundary, or a Nx3 matrix with surface
%                      points
%
% See also FT_SOURCEANALYSIS, FT_MEGREALIGN, FT_DIPOLEFITTING, FT_PREPARE_LEADFIELD

% Searching through the code, it seems that the following cfg fields are being used
% cfg.grid
% cfg.mri
% cfg.headshape
% cfg.tightgrid
% cfg.symmetry
% cfg.smooth
% cfg.threshold
% cfg.spheremesh
% cfg.inwardshift
% cfg.mriunits
% cfg.sourceunits

% Copyright (C) 2004-2011, Robert Oostenveld
%
% %Log$

% set the defaults
if ~isfield(cfg, 'symmetry'),         cfg.symmetry    = [];       end
if ~isfield(cfg, 'grid'),             cfg.grid        = [];       end

if ~isfield(cfg, 'vol') && nargin>1
  % put it in the configuration structure
  % this is for backward compatibility, 13 Januari 2011
  cfg.vol = vol;
end

if ~isfield(cfg, 'sens') && nargin>2
  % put it in the configuration structure
  % this is for backward compatibility, 13 Januari 2011
  cfg.grad = sens;
end

% for backward compatibility
if isfield(cfg, 'tightgrid')
  cfg.grid.tight = cfg.tightgrid;
  cfg = rmfield(cfg, 'tightgrid');
end

if isfield(cfg.grid, 'resolution') && isfield(cfg.grid, 'xgrid') && ~ischar(cfg.grid.xgrid)
  error('You cannot specify cfg.grid.resolution and an explicit cfg.grid.xgrid simultaneously');
end
if isfield(cfg.grid, 'resolution') && isfield(cfg.grid, 'ygrid') && ~ischar(cfg.grid.ygrid)
  error('You cannot specify cfg.grid.resolution and an explicit cfg.grid.ygrid simultaneously');
end
if isfield(cfg.grid, 'resolution') && isfield(cfg.grid, 'zgrid') && ~ischar(cfg.grid.zgrid)
  error('You cannot specify cfg.grid.resolution and an explicit cfg.grid.zgrid simultaneously');
end

% a grid can be constructed based on a number of ways
basedonauto   = isfield(cfg.grid, 'resolution');                        % regular 3D grid with specification of the resolution
basedongrid   = isfield(cfg.grid, 'xgrid') && ~ischar(cfg.grid.xgrid);  % regular 3D grid with explicit specification
basedonpos    = isfield(cfg.grid, 'pos');                               % using user-supplied grid positions, which can be regular or irregular
basedonshape  = isfield(cfg, 'headshape') && ~isempty(cfg.headshape);   % surface grid based on inward shifted head surface from external file
basedonmri    = isfield(cfg, 'mri');                                    % regular 3D grid, based on segmented MRI, restricted to gray matter
basedonvol    = false;                                                  % surface grid based on inward shifted brain surface from volume conductor
basedoncortex = isfield(cfg, 'headshape') && (iscell(cfg.headshape) || ft_filetype(cfg.headshape, 'neuromag_fif') || ft_filetype(cfg.headshape, 'freesurfer_triangle_binary')); % cortical sheet from MNE or Freesurfer, also in case of multiple files/hemispheres

if basedonshape && basedoncortex
  % treating it as cortical sheet has preference
  basedonshape = false;
end

if basedongrid && basedonpos
  % fall back to default behaviour, in which the pos overrides the grid
  basedongrid = false;
end

if ~any([basedonauto basedongrid basedonpos basedonshape basedonmri basedoncortex]) && nargin>1 && ~isempty(cfg.vol)
  % fall back to default behaviour, which is to create a surface grid (e.g. used in MEGRELIGN)
  basedonvol = 1;
end

% these are mutually exclusive, but printing all requested methods here
% facilitates debugging of weird configs. Also specify the defaults here to
% keep the overview
if basedonauto
  fprintf('creating dipole grid based on automatic 3D grid with specified resolution\n');
  if ~isfield(cfg.grid, 'xgrid'),       cfg.grid.xgrid = 'auto';    end
  if ~isfield(cfg.grid, 'ygrid'),       cfg.grid.ygrid = 'auto';    end
  if ~isfield(cfg.grid, 'zgrid'),       cfg.grid.zgrid = 'auto';    end
  if ~isfield(cfg, 'inwardshift'),      cfg.inwardshift = 0;        end % in this case for inside detection, % FIXME move to cfg.grid
  if ~isfield(cfg.grid, 'tight'),       cfg.grid.tight = 'yes';     end
end

if basedongrid
  fprintf('creating dipole grid based on user specified 3D grid\n');
  if ~isfield(cfg, 'inwardshift'),      cfg.inwardshift = 0;        end % in this case for inside detection, % FIXME move to cfg.grid
  if ~isfield(cfg.grid, 'tight'),       cfg.grid.tight = 'no';      end
end

if basedonpos
  fprintf('creating dipole grid based on user specified dipole positions\n');
  if ~isfield(cfg, 'inwardshift'),      cfg.inwardshift = 0;        end % in this case for inside detection, % FIXME move to cfg.grid
  if ~isfield(cfg.grid, 'tight'),       cfg.grid.tight = 'no';      end
end

if basedonshape
  fprintf('creating dipole grid based on inward-shifted head shape\n');
  if ~isfield(cfg, 'spheremesh'),       cfg.spheremesh = 642;  end  % FIXME move spheremesh to cfg.grid
  if ~isfield(cfg.grid, 'tight'),       cfg.grid.tight = 'no'; end
end

if basedoncortex
  if ~isfield(cfg, 'sourceunits');      cfg.sourceunits = 'cm';     end
  if ~isfield(cfg.grid, 'tight'),       cfg.grid.tight = 'no'; end
end

if basedonmri
  fprintf('creating dipole grid based on grey matter from segmented MRI\n');
  if ~isfield(cfg, 'threshold'),        cfg.threshold = 0.1;        end % relative
  if ~isfield(cfg, 'smooth');           cfg.smooth    = 5;          end % in voxels
  if ~isfield(cfg, 'sourceunits');      cfg.sourceunits = 'cm';     end
  if ~isfield(cfg, 'mriunits');         cfg.mriunits = 'mm';        end
  if ~isfield(cfg.grid, 'tight'),       cfg.grid.tight = 'yes';     end
end

if basedonvol
  fprintf('creating dipole grid based on inward-shifted brain surface from volume conductor model\n');
  if ~isfield(cfg, 'spheremesh'),       cfg.spheremesh = 642;  end   % FIXME move to cfg.grid
  if ~isfield(cfg.grid, 'tight'),       cfg.grid.tight = 'no'; end
end

% these are mutually exclusive
if sum([basedonauto basedongrid basedonpos basedonshape basedonmri basedonvol basedoncortex])~=1
  error('incorrect cfg specification for constructing a dipole grid');
end

needspm = isfield(cfg, 'smooth') && ~strcmp(cfg.smooth, 'no');
if needspm
  % check if SPM is in path, do not add any of them to the path yet
  hasspm2 = ft_hastoolbox('SPM2');
  hasspm8 = ft_hastoolbox('SPM8');

  if ~hasspm2 && ~hasspm8
    % is neither is on the path, try adding SPM8
    try, hasspm8 = ft_hastoolbox('SPM8', 1); end
  end

  if ~hasspm8
    % if SPM8 could not be added, try adding SPM2
    try, ft_hastoolbox('SPM2', 1); end
  end
end

% start with an empty grid
grid = [];

% copy the volume conductor and sensor array out of the cfg
if isfield(cfg, 'vol')
  vol = cfg.vol;
else
  vol = [];
end

% these are mutually exclusive
if isfield(cfg, 'grad')
  sens = cfg.grad;
elseif isfield(cfg, 'elec')
  sens = cfg.elec;
else
  sens = [];
end

if basedonauto
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % construct a regular 3D grid that spans a box encompassing all electrode
  % or gradiometer coils, this will typically also cover the complete brain
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  if ischar(cfg.grid.xgrid) && strcmp(cfg.grid.xgrid, 'auto')
    grid.xgrid = floor(min(sens.pnt(:,1))):cfg.grid.resolution:ceil(max(sens.pnt(:,1)));
  end
  if ischar(cfg.grid.ygrid) && strcmp(cfg.grid.ygrid, 'auto')
    grid.ygrid = floor(min(sens.pnt(:,2))):cfg.grid.resolution:ceil(max(sens.pnt(:,2)));
  end
  if ischar(cfg.grid.zgrid) && strcmp(cfg.grid.zgrid, 'auto')
    grid.zgrid = floor(min(sens.pnt(:,3))):cfg.grid.resolution:ceil(max(sens.pnt(:,3)));
  end
  grid.dim   = [length(grid.xgrid) length(grid.ygrid) length(grid.zgrid)];
  [X, Y, Z]  = ndgrid(grid.xgrid, grid.ygrid, grid.zgrid);
  grid.pos   = [X(:) Y(:) Z(:)];
end

if basedongrid
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % a detailled xgrid/ygrid/zgrid has been specified, the other details
  % still need to be determined
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  grid.xgrid = cfg.grid.xgrid;
  grid.ygrid = cfg.grid.ygrid;
  grid.zgrid = cfg.grid.zgrid;
  grid.dim   = [length(grid.xgrid) length(grid.ygrid) length(grid.zgrid)];
  [X, Y, Z]  = ndgrid(grid.xgrid, grid.ygrid, grid.zgrid);
  grid.pos   = [X(:) Y(:) Z(:)];
end

if basedonpos
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % a grid is already specified in the configuration, reuse as much of the
  % prespecified grid as possible (but only known objects)
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  if isfield(cfg.grid, 'pos')
    grid.pos = cfg.grid.pos;
  end
  if isfield(cfg.grid, 'mom')
    grid.mom = cfg.grid.mom;
  end
  if isfield(cfg.grid, 'dim')
    grid.dim = cfg.grid.dim;
  end
  if isfield(cfg.grid, 'xgrid')
    % FIXME is it desirable to have this in the grid?
    grid.xgrid = cfg.grid.xgrid;
  end
  if isfield(cfg.grid, 'ygrid')
    % FIXME is it desirable to have this in the grid?
    grid.ygrid = cfg.grid.ygrid;
  end
  if isfield(cfg.grid, 'zgrid')
    % FIXME is it desirable to have this in the grid?
    grid.zgrid = cfg.grid.zgrid;
  end
  if isfield(cfg.grid, 'dim')
    grid.dim = cfg.grid.dim;
  elseif isfield(grid, 'xgrid') && isfield(grid, 'ygrid') && isfield(grid, 'zgrid')
    grid.dim = [length(grid.xgrid) length(grid.ygrid) length(grid.zgrid)];
  end
  if isfield(cfg.grid, 'inside')
    grid.inside = cfg.grid.inside;
  end
  if isfield(cfg.grid, 'outside')
    grid.outside = cfg.grid.outside;
  end
  if isfield(cfg.grid, 'lbex')
    grid.lbex = cfg.grid.lbex;
  end
  if isfield(cfg.grid, 'subspace')
    grid.subspace = cfg.grid.subspace;
  end
  if isfield(cfg.grid, 'leadfield')
    grid.leadfield = cfg.grid.leadfield;
  end
  if isfield(cfg.grid, 'filter')
    grid.filter = cfg.grid.filter;
  end
  if isfield(cfg.grid, 'tight')
    grid.tight = cfg.grid.tight;
  end
  % this is not supported any more
  if isfield(cfg.grid, 'avg') && isfield(cfg.grid.avg, 'filter')
    error('please put your filters in cfg.grid instead of cfg.grid.avg');
  end
end

if basedonmri
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % construct a grid based on the segmented MRI that is provided in the
  % configuration, only voxels in gray matter will be used
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % convert the source/functional data into the same units as the anatomical MRI
  scale = 1;
  switch cfg.sourceunits
    case 'mm'
      scale = scale / 1000;
    case 'cm'
      scale = scale / 100;
    case 'dm'
      scale = scale / 10;
    case 'm'
      scale = scale / 1;
    otherwise
      error('unknown physical dimension in cfg.sourceunits');
  end
  switch cfg.mriunits
    case 'mm'
      scale = scale * 1000;
    case 'cm'
      scale = scale * 100;
    case 'dm'
      scale = scale * 10;
    case 'm'
      scale = scale * 1;
    otherwise
      error('unknown physical dimension in cfg.mriunits');
  end

  if ~isfield(cfg.grid, 'resolution')
    switch cfg.sourceunits
      case 'mm'
        cfg.grid.resolution = 10;
      case 'cm'
        cfg.grid.resolution = 1;
      case 'dm'
        cfg.grid.resolution = 0.1;
      case 'm'
        cfg.grid.resolution = 0.01;
    end
  end

  if ischar(cfg.mri)
    % read the segmentation from file
    mri      = ft_read_mri(cfg.mri);
    mri.gray = double(mri.anatomy);
  elseif isstruct(cfg.mri) && ~isfield(cfg.mri, 'gray')
    % looks like a segmentation that has already been loaded in memory
    mri      = cfg.mri;
    mri.gray = double(mri.anatomy);
  elseif isstruct(cfg.mri) && isfield(cfg.mri, 'gray')
    % looks like a complete segmentation from VOLUMESEGMENT
    mri      = cfg.mri;
    mri.gray = double(mri.gray);
  else
    error('cannot determine the format of the segmentation in cfg.mri');
  end

  % apply a smoothing of a certain amount of voxels
  if ~strcmp(cfg.smooth, 'no');
    fprintf('smoothing gray matter segmentation with %d voxels\n', cfg.smooth);
    spm_smooth(mri.gray, mri.gray, cfg.smooth);
  end

  % determine for each voxel whether it belongs to the cortex
  if isfield(cfg, 'threshold')
    fprintf('thresholding gray matter segmentation at a relative value of %f\n', cfg.threshold);
    head = mri.gray./max(mri.gray(:)) > cfg.threshold;
  else
    error('you must specify cfg.threshold for cortex segmentation');
  end

  ind                 = find(head(:));
  fprintf('%d from %d voxels in the segmentation are marked as cortex (%.0f%%)\n', length(ind), prod(size(head)), 100*length(ind)/prod(size(head)));
  [X,Y,Z]             = ndgrid(1:mri.dim(1), 1:mri.dim(2), 1:mri.dim(3));   % create the grid in MRI-coordinates
  posmri              = [X(ind) Y(ind) Z(ind) ones(length(ind),1)];         % take only the inside voxels
  poshead             = mri.transform * posmri';                            % transform to head coordinates
  poshead             = poshead(1:3,:)';
  posmri              = posmri(:,1:3);
  resolution          = cfg.grid.resolution*scale;                                        % source and mri can be expressed in different units (e.g. cm and mm)
  xgrid               = floor(min(poshead(:,1))):resolution:ceil(max(poshead(:,1)));      % create the grid in head-coordinates
  ygrid               = floor(min(poshead(:,2))):resolution:ceil(max(poshead(:,2)));      % with 'consistent' x,y,z definitions
  zgrid               = floor(min(poshead(:,3))):resolution:ceil(max(poshead(:,3)));
  [X,Y,Z]             = ndgrid(xgrid,ygrid,zgrid);
  pos2head            = [X(:) Y(:) Z(:) ones(length(X(:)),1)]';
  pos2mri             = mri.transform \ pos2head;                                         % transform to MRI-coordinates
  pos2mri             = round(pos2mri(1:3,:))';
  pos2head            = pos2head(1:3,:)';
  pos2mri             = pos2mri(:,1:3);
  % it might be that the box with the points does not completely fit into the MRI
  sel = find(pos2mri(:,1)<1 |  pos2mri(:,1)>size(head,1) | ...
    pos2mri(:,2)<1 |  pos2mri(:,2)>size(head,2) | ...
    pos2mri(:,3)<1 |  pos2mri(:,3)>size(head,3));
  if isempty(sel)
    % use the efficient implementation
    inside = head(sub2ind(mri.dim, pos2mri(:,1), pos2mri(:,2), pos2mri(:,3)));
  else
    % only loop over the points that can be dealt with
    inside = zeros(length(xgrid)*length(ygrid)*length(zgrid), 1);
    for i=setdiff(1:size(pos2mri,1), sel(:)')
      inside(i) = head(pos2mri(i,1), pos2mri(i,2), pos2mri(i,3));
    end
  end
  inside = find(inside);

  grid.pos            = pos2head/scale;                                     % convert to source units
  grid.xgrid          = xgrid/scale;                                        % convert to source units
  grid.ygrid          = ygrid/scale;                                        % convert to source units
  grid.zgrid          = zgrid/scale;                                        % convert to source units
  grid.dim            = [length(grid.xgrid) length(grid.ygrid) length(grid.zgrid)];
  grid.inside         = inside(:);
  grid.outside        = setdiff(1:size(grid.pos,1),grid.inside)';

  fprintf('the regular 3D grid encompassing the cortex contains %d grid points\n', size(grid.pos,1));
  fprintf('%d grid points inside gray matter\n', length(grid.inside));
  fprintf('%d grid points outside gray matter\n', length(grid.outside));
end

if basedoncortex
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % read it from a *.fif file that was created using Freesurfer and MNE
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  if iscell(cfg.headshape)
    % FIXME loop over all files, this should be two hemispheres
    keyboard
  end
  switch ft_filetype(cfg.headshape)
    case 'freesurfer_triangle_binary'
      % it contains a cortical sheet which was created by the Freesurfer software
      shape = ft_read_headshape(cfg.headshape, 'format', 'freesurfer_triangle_binary');
    case 'neuromag_fif'
      % fif files can contain a variety of objects
      % here we assume that it contains a cortical sheet which was created by the MNE software
      shape = ft_read_headshape(cfg.headshape, 'format', 'mne_source');
    otherwise
      % use autodetection
      shape = ft_read_headshape(cfg.headshape);
  end
  grid.pos = shape.pnt;
  grid.tri = shape.tri;
  grid = ft_convert_units(grid, cfg.sourceunits);
end

if basedonshape
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % use the headshape  to make a superficial dipole layer (e.g.
  % for megrealign). Assume that all points are inside the volume.
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % get the surface describing the head shape
  if isstruct(cfg.headshape) && isfield(cfg.headshape, 'pnt')
    % use the headshape surface specified in the configuration
    headshape = cfg.headshape;
  elseif isnumeric(cfg.headshape) && size(cfg.headshape,2)==3
    % use the headshape points specified in the configuration
    headshape.pnt = cfg.headshape;
  elseif ischar(cfg.headshape)
    % read the headshape from file
    headshape = ft_read_headshape(cfg.headshape);
  else
    error('cfg.headshape is not specified correctly')
  end
  if ~isfield(headshape, 'tri')
    % generate a closed triangulation from the surface points
    headshape.pnt = unique(headshape.pnt, 'rows');
    headshape.tri = projecttri(headshape.pnt);
  end
  grid.pos = headsurface([], [], 'headshape', headshape, 'inwardshift', cfg.inwardshift, 'npnt', cfg.spheremesh);
  grid.inside  = 1:size(grid.pos,1);
  grid.outside = [];
end

if basedonvol
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % use the volume conduction model to make a superficial dipole layer (e.g.
  % for megrealign). Assume that all points are inside the volume.
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  grid.pos = headsurface(vol, sens, 'inwardshift', cfg.inwardshift, 'npnt', cfg.spheremesh);
  grid.inside  = 1:size(grid.pos,1);
  grid.outside = [];
end

% FIXME use inside_vol instead of this replication of code
% determine the dipole locations inside the brain volume
if ~isfield(grid, 'inside') && ~isfield(grid, 'outside')
  if ft_voltype(vol, 'infinite') || ft_voltype(vol, 'halfspace')
    % an empty vol in combination with gradiometers indicates a magnetic dipole
    % in an infinite vacuum, i.e. all dipoles can be considered to be inside
    grid.inside = 1:size(grid.pos,1);
    grid.outside = [];
    outside = zeros(1,size(grid.pos,1));
    for i =1:size(grid.pos,1);
      invacuum = false;
      dip1 = grid.pos(i,:);
      % condition of dipoles falling in the non conductive halfspace
      invacuum = acos(dot(vol.ori,(dip1-vol.pnt)./norm(dip1-vol.pnt))) < pi/2;
      if invacuum
        outside(i) = 1;
      end
    end
    grid.outside = find(outside);
    grid.inside  = find(~outside);
  else
    if isfield(sens, 'ori') && isfield(sens, 'pnt') && isfield(sens, 'tra')
      % in case of MEG, make a triangulation of the outermost surface
      if isfield(cfg, 'headshape')
        % use the specified headshape to construct the bounding triangulation
        [pnt, tri] = headsurface(vol, sens, 'headshape', cfg.headshape, 'inwardshift', cfg.inwardshift, 'surface', 'skin');
      else
        % use the volume conductor model to construct the bounding triangulation
        [pnt, tri] = headsurface(vol, sens, 'inwardshift', cfg.inwardshift, 'surface', 'skin');
      end
    else
      % in case of EEG, make a triangulation of the innermost surface
      [pnt, tri] = headsurface(vol, sens, 'inwardshift', cfg.inwardshift, 'surface', 'brain');
    end
    % determine the dipole positions that are inside the triangulated surface
    tmp = bounding_mesh(grid.pos, pnt, tri);
    grid.inside  = find(tmp==1);
    grid.outside = find(tmp==0);
  end
elseif ~isfield(grid, 'inside')
  grid.inside = setdiff(1:size(grid.pos,1), grid.outside);
elseif ~isfield(grid, 'outside')
  grid.outside = setdiff(1:size(grid.pos,1), grid.inside);
end

if strcmp(cfg.grid.tight, 'yes')
  fprintf('%d dipoles inside, %d dipoles outside brain\n', length(grid.inside), length(grid.outside));
  fprintf('making tight grid\n');
  xmin = min(grid.pos(grid.inside,1));
  ymin = min(grid.pos(grid.inside,2));
  zmin = min(grid.pos(grid.inside,3));
  xmax = max(grid.pos(grid.inside,1));
  ymax = max(grid.pos(grid.inside,2));
  zmax = max(grid.pos(grid.inside,3));
  xmin_indx = find(grid.xgrid==xmin);
  ymin_indx = find(grid.ygrid==ymin);
  zmin_indx = find(grid.zgrid==zmin);
  xmax_indx = find(grid.xgrid==xmax);
  ymax_indx = find(grid.ygrid==ymax);
  zmax_indx = find(grid.zgrid==zmax);
  sel =       (grid.pos(:,1)>=xmin & grid.pos(:,1)<=xmax); % select all grid positions inside the tight box
  sel = sel & (grid.pos(:,2)>=ymin & grid.pos(:,2)<=ymax); % select all grid positions inside the tight box
  sel = sel & (grid.pos(:,3)>=zmin & grid.pos(:,3)<=zmax); % select all grid positions inside the tight box
  grid.pos   = grid.pos(sel,:);
  % update the inside and outside vector
  tmp = zeros(1,prod(grid.dim));
  tmp(grid.inside)  = 1;        % these are originally inside the brain
  tmp(grid.outside) = 0;        % these are originally outside the brain
  tmp               = tmp(sel); % within the tight box, these are inside the brain
  grid.inside  = find(tmp);
  grid.outside = find(~tmp);
  grid.xgrid   = grid.xgrid(xmin_indx:xmax_indx);
  grid.ygrid   = grid.ygrid(ymin_indx:ymax_indx);
  grid.zgrid   = grid.zgrid(zmin_indx:zmax_indx);
  grid.dim     = [length(grid.xgrid) length(grid.ygrid) length(grid.zgrid)];
end
fprintf('%d dipoles inside, %d dipoles outside brain\n', length(grid.inside), length(grid.outside));

% apply the symmetry constraint, i.e. add a symmetric dipole for each location defined sofar
% set up the symmetry constraints
if ~isempty(cfg.symmetry)
  if strcmp(cfg.symmetry, 'x')
    reduce = [1 2 3];         % select the parameters [x1 y1 z1]
    expand = [1 2 3 1 2 3];   % repeat them as [x1 y1 z1 x1 y1 z1]
    mirror = [1 1 1 -1 1 1];  % multiply each of them with 1 or -1, resulting in [x1 y1 z1 -x1 y
  elseif strcmp(cfg.symmetry, 'y')
    reduce = [1 2 3];         % select the parameters [x1 y1 z1]
    expand = [1 2 3 1 2 3];   % repeat them as [x1 y1 z1 x1 y1 z1]
    mirror = [1 1 1 1 -1 1];  % multiply each of them with 1 or -1, resulting in [x1 y1 z1 x1 -y
  elseif strcmp(cfg.symmetry, 'z')
    reduce = [1 2 3];         % select the parameters [x1 y1 z1]
    expand = [1 2 3 1 2 3];   % repeat them as [x1 y1 z1 x1 y1 z1]
    mirror = [1 1 1 1 1 -1];  % multiply each of them with 1 or -1, resulting in [x1 y1 z1 x1 y1
  else
    error('unrecognized symmetry constraint');
  end
  fprintf('each source describes two dipoles with symmetry along %s axis\n', cfg.symmetry);
  % expand the number of parameters from one (3) to two dipoles (6)
  grid.pos = grid.pos(:,expand) .* repmat(mirror, size(grid.pos,1), 1);
end

% FIXME should the cfg be added to the output grid?


function [sDataPET] = correctPatient24_HN(pathDATA)
% -------------------------------------------------------------------------
% function [sDataPET] = correctPatient24_HN(pathDATA)
% -------------------------------------------------------------------------
% DESCRIPTION:
% Reason of existence of the function: the RTstruct of the PET scan of
% patient 24 is incomplete (most probably it was previously incorrectly
% processed by MIM). The problem is solved by copying and processing the
% 'contour' structure of the CT scan.
% -------------------------------------------------------------------------
% INPUTS:
% - pathDATA: Full path to the 'DATA' folder of the HN workspace.
% -------------------------------------------------------------------------
% OUTPUTS:
% - sData: Corrected sData.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: July 2015
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

startpath = pwd;
cd(pathDATA)
load('Patient24_CT'), sDataCT = sData;
load('Patient24_PET'), sDataPET = sData;

% ADJUSTING PET CONTOURS ACCORDING TO CT CONTOURS.
sDataPET{2}.scan.contour = sDataCT{2}.scan.contour;
nContour = length(sDataPET{2}.scan.contour);
downF = sDataPET{2}.scan.pixelW/sDataCT{2}.scan.pixelW;
offsetStart = round(abs(sDataPET{3}(1).ImagePositionPatient(2)-sDataCT{3}(1).ImagePositionPatient(2))*sDataCT{2}.scan.pixelW);
offsetEnd = round((size(sDataCT{2}.scan.volume,1)*sDataCT{2}.scan.pixelW - size(sDataPET{2}.scan.volume,1)*sDataPET{2}.scan.pixelW)/sDataCT{2}.scan.pixelW)-offsetStart;
for i = 1:nContour
    sDataPET{2}.scan.contour(i).boxBound(1:2,1) = round((sDataCT{2}.scan.contour(i).boxBound(1:2,1) - offsetStart)/downF); sDataPET{2}.scan.contour(i).boxBound(1:2,2) = round((sDataCT{2}.scan.contour(i).boxBound(1:2,2) - offsetEnd)/downF);
    boxBound = sDataPET{2}.scan.contour(i).boxBound;
    nSlices = size(sDataPET{2}.scan.contour(i).boxMask,3);
    sDataPET{2}.scan.contour(i).boxMask = zeros(boxBound(1,2)-boxBound(1,1)+1,boxBound(2,2)-boxBound(2,1)+1,nSlices);
    for j = 1:nSlices
        sDataPET{2}.scan.contour(i).boxMask(:,:,j) = imresize(sDataCT{2}.scan.contour(i).boxMask(:,:,j),[boxBound(1,2)-boxBound(1,1)+1,boxBound(2,2)-boxBound(2,1)+1],'nearest');
    end
end

cd(startpath)
end


%GUI script for reading ctf data into eeglab.
%to run type ctf2eeglab on the command line.

clear all;
folder = uigetdir('*.ds','Data Set Finder');
[ctf2] = ctf_read_res4(folder,1);
sensors = {'meg','eeg','ref','vc'};
sensloc = 'ctf2.sensor.index';
sensorlen = '1:';
num2str(length(ctf2.sensor.index.meg_sens));

k = menu('Choose the channels you would like to use:','MEG','EEG','Reference','Virtual');
sensloc = strcat(sensloc,sensors(k));
sensorlen = strcat(sensorlen, num2str(length(eval(sensloc{1}))));
prompt   = {'Enter Sensor Numbers to Read (ie 2:7 or 1 3 7 23 65)'};
title    = 'Input for Sensor Range (Default is all sensors)';
lines = 1;
def     = {sensorlen};
sensnum   = inputdlg(prompt,title,lines,def);
for i = str2num(sensnum{1});
    if ~ismember(str2num(def{1}),i)
        errordlg('There is no such sensor','Sensor Range Error')
        exit(1);
    end
end

trialnums = '1:';
trialnums = strcat(trialnums, num2str(ctf2.setup.number_trials));
prompt2 = {'Enter the trials you would like to use(ie 1:3 or 2 4 5):'};
def2 = {trialnums};
trials = inputdlg(prompt2, 'Input for trial range', 1,def2);
for i = str2num(trials{1})
    if ~ismember(str2num(def2{1}),i)
        errordlg('Trial not in dataset','Trial Error')
        exit(1);
    end
end
button = questdlg('Would you like to use markers?','Use Markers?','Yes','No','Yes');
if strcmp(button,'Yes')
    [marker_info] = readmarkerfile(folder);
    markers = marker_info.marker_names;
    m = menu('Please choose which marker you would like to use', markers);
end
prompt   = {'Enter the start time (sec):', 'Enter the end time:'};
title    = 'Time Window';
lines = 1;
def     = {'0', '.15'};
def(1) = {num2str(ctf2.setup.start_sec)};
def(2) = {num2str(ctf2.setup.end_sec)};
markeranswer = inputdlg(prompt,title,lines,def);
wind = [0 0];
wind(1) = str2num(markeranswer{1});
wind(2) = str2num(markeranswer{2});

if strcmp(button,'Yes')
    [data]=readepochs(folder,'marker_info',marker_info,'ctf',ctf2,sensors{k}, str2num(sensnum{1}),'trials',str2num(trials{1}), 'markers',markers(m),'window', wind);
else
    [data]=readepochs(folder,'ctf',ctf2,sensors{k}, str2num(sensnum{1}),'trials',str2num(trials{1}), 'window', wind);
end

%wind = [-.1 .15];
%[data]=readepochs(folder,'markers',{'click'},'window',wind);
%[data]=readepochs(folder,'window',wind,'trials',[1:10],'megsens',[2:7]);

dat=permute(data.epochs{1},[2,1,3]);
size(dat)
[ALLEEG EEG CURRENTSET ALLCOM] = eeglab;

EEG = pop_importdata( 'nbchan', size(dat,1), 'dataformat', 'array', 'data', 'dat', 'pnts', size(dat,2), 'srate', data.setup.sample_rate, 'xmin', wind(1));
[ALLEEG EEG CURRENTSET] = pop_newset(ALLEEG, EEG, CURRENTSET,  'setname',data.setup.subject);
eeglab redraw;

for i=1:size(dat,1)
    if ~isempty(data.sensor.location)
        EEG.chanlocs(i).labels=char(data.sensor.label(i));
        EEG.chanlocs(i).X=data.sensor.location(1,i);
        EEG.chanlocs(i).Y=data.sensor.location(2,i);
        EEG.chanlocs(i).Z=data.sensor.location(3,i);
    end
end
EEG.chanlocs=pop_chanedit(EEG.chanlocs,  'convert', 'cart2topo', 'convert', 'cart2sph');
[ALLEEG EEG] = eeg_store(ALLEEG, EEG, CURRENTSET);
eeglab redraw;
clear dat data ctf2 marker_info markers sensors;


function val = viewGetVolume(vw,param,varargin)
% Get data from various view structures
%
% This function is wrapped by viewGet. It should not be called by anything
% else other than viewGet.
%
% This function retrieves information from the view that relates to a
% specific component of the application.
%
% We assume that input comes to us already fixed and does not need to be
% formatted again.

if notDefined('vw'), vw = getCurView; end
if notDefined('param'), error('No parameter defined'); end

mrGlobals;
val = [];

switch param

    case 'nodes'
        % Return the array of nodes. Only gray views have nodes. See help
        % for mrManDist.m for a description of the node structure. In
        % brief, nodes are 8 x nvoxels. The first 3 rows correspond to the
        % voxel location and the next 5 correspond to gray graph-related
        % data.
        %   nodes = viewGet(vw, 'nodes');
        if isfield(vw, 'nodes'), val = vw.nodes;
        else
            val = [];
            warning('vista:viewError', 'Nodes not found.');
        end
    case 'xyznodes'
        % Return the xyz coordinates of the gray voxels as found in nodes
        % array. Assumes a Gray view. See case 'nodes' and help for
        % mrManDist for more information.
        %
        % Must call this sagittal, axial coronal or whatever the mapping is
        % ras, 06/07 -- I believe it's [cor axi sag]. coords is [axi cor sag].
        %
        % xyzNodes = viewGet(vw, 'xyz nodes');
        nodes = viewGet(vw,'nodes');
        val = nodes(1:3,:);
    case 'nodegraylevel'
        % Return the gray level of each voxel as determined by the nodes
        % array. Assumes a Gray view. See case 'nodes' and help for
        % mrManDist for more information.
        %   nodeGrayLevel = viewGet(vw, 'gray level');
        nodes = viewGet(vw,'nodes');
        val = nodes(6,:);
    case 'nnodes'
        % Return the number of nodes. Assumes a Gray view. See case 'nodes'
        % and help for mrManDist for more information.
        %   nNodes = viewGet(vw, 'number of nodes');
        val = size( viewGet(vw, 'nodes'), 2 );
    case 'edges'
        % Return the edge structure of the gray graph. Assumes a Gray view.
        % See help for mrManDist for more information.
        %   edges = viewGet(vw, 'edges');
        if isfield(vw, 'edges'), val = vw.edges;
        else val = []; warning('vista:viewError', 'Edges not found.'); end
    case 'nedges'
        % Return the number of edges in the gray graph. Assumes a Gray
        % view. See case 'edges' and help for mrManDist for more
        % information.
        %   nEdges = viewGet(vw, 'number of edges');
        val = length( viewGet(vw, 'edges') );
    case 'allleftnodes'
        % Return the subset of nodes in the Gray graph that are in the left
        % hemisphere. See mrgGrowGray and mrManDist.
        %   allLeftNodes = viewGet(vw, 'all left nodes');
        val = vw.allLeftNodes;
    case 'allleftedges'
        % Return the subset of edges in the Gray graph that are in the left
        % hemisphere. See mrgGrowGray and mrManDist.
        %   allLeftEdges = viewGet(vw, 'all left edges');
        val = vw.allLeftEdges;
        if isempty(val) && Check4File('Gray/coords')
            % Try laoding from the Gray/coords file
            load('Gray/coords', 'allLeftEdges')
            val = allLeftEdges;
        end
    case 'allrightnodes'
        % Return the subset of nodes in the Gray graph that are in the
        % right hemisphere. See mrgGrowGray and mrManDist.
        %   allRightNodes = viewGet(vw, 'all right nodes');
        val = vw.allRightNodes;
        if isempty(val) && Check4File('Gray/coords')
            % Try laoding from the Gray/coords file
            load('Gray/coords', 'allRightNodes')
            val = allRightNodes;
        end

    case 'allrightedges'
        % Return the subset of edges in the Gray graph that are in the
        % right hemisphere. See mrgGrowGray and mrManDist.
        %   allRightEdges = viewGet(vw, 'all right edges');
        val = vw.allRightEdges;

    case 'allnodes'
        % Return all nodes from Gray graph by taking union of allLeftNodes
        % and allRightNodes.
        %
        % This is NOT necessarily the same as simply returning 'vw.nodes'.
        % When we install a segmentation, we can either keep all the nodes
        % in the gray graph, or only those that fall within the functional
        % field of view (to save space). When we do the latter, the fields
        % vw.coords, vw.nodes, and vw.edges contain only the coords, nodes,
        % and eges within the functional field of view. However the fields
        % vw.allLeftNodes, vw.allLeftEdges, vw.allRightNodes, and
        % vw.allRightEdges contain the edges and nodes for the entire
        % hemisphere
        %
        % Example: nodes = viewGet(vw, 'all nodes');
        val = [vw.allLeftNodes'; vw.allRightNodes']';

    case 'alledges'
        % Return all edges from Gray graph by taking union of allLeftEdges
        % and allRightEdges. See 'allnodes' for explanation.
        %
        % Example: edges = viewGet(vw, 'all edges');
        val = [vw.allLeftEdges vw.allRightEdges];

    case 'coords'
        % Return all the coordinates in the current view. If in Flat view,
        % return the coordinates for a particular slice (slice specified in
        % varargin{1}). If in Inplane view, slice specification is
        % optional. If in Gray or Volume view, slice specification is
        % ignored.
        %   <gray, volume or inplane>
        %       coords = viewGet(vw, 'coords');
        %   <flat or inplane>
        %       slice  = viewGet(vw, 'current slice');
        %       coords = viewGet(vw,'coords', slice);
        try
            switch lower(viewGet(vw, 'viewType'))
                case 'flat'
                    %% separate coords for each flat hemisphere
                    if length(varargin) ~= 1
                        error('You must specify which hemisphere.');
                    end
                    hname = varargin{1};
                    switch hname
                        case 'left'
                            val = vw.coords{1};
                        case 'right'
                            val = vw.coords{2};
                        otherwise
                            error('Bad hemisphere name');
                    end
                case {'gray', 'volume', 'hiddengray'}
                    val = vw.coords;
                case 'inplane'
                    % These coords are for inplane anat. Functional coords
                    % may have different values (if
                    % upSampleFactor(vw,scan) ~= 1)
                    dims = viewGet(vw, 'anatomysize');
                    if length(varargin) >= 1  % then we want coords for just one slice
                        slice = varargin{1};
                        indices = 1+prod([dims(1:2) slice-1]):prod([dims(1:2) slice]);
                        val=indices2Coords(indices,dims);
                    else
                        indices = 1:prod(dims);
                        val=indices2Coords(indices,dims);
                    end
            end
        catch ME
            val=[];
            warning(ME.identifier, ME.message);
            fprintf('[%s]: Coords not found.', mfilename);
        end

    case 'allcoords'
        % Return all coords from Gray graph, including those that are not
        % included in the functional field of view. See 'allnodes' for
        % explanation. If session was initialized with the option
        % 'keepAllNodes' == true, then this call will be identical to
        % viewGet(vw.coords).
        %
        % Optional input: hemi ('right' 'left' 'both') for right, left or both
        %
        % Example: coords = viewGet(vw, 'allcoords', 'right');
        nodes = viewGet(vw, 'nodes');

        if length(varargin) ~= 1
            hname = 'both';
        else
            hname = varargin{1};
        end

        switch hname
            case 'left'
                subnodes  = viewGet(vw, 'all left nodes');
                [~, ia] = intersectCols(nodes(1:3,:), subnodes(1:3,:));
            case 'right'
                subnodes  = viewGet(vw, 'all right nodes');
                [~, ia] = intersectCols(nodes(1:3,:), subnodes(1:3,:));
            otherwise
                ia = 1:size(nodes,2);
        end

        val = nodes([2 1 3], ia);

    case 'coordsfilename'
        % Return the path to the file in which coordinates are stored.
        % Assumes that a gray view has been created (though current view
        % can be any type).
        %   coordsFileName = viewGet(vw, 'coords file name');
        homeDir = viewGet(vw, 'homedir');
        if isempty(homeDir), val = ['Gray' filesep 'coords.mat'];
        else val = [homeDir filesep 'Gray' filesep 'coords.mat'];
        end
    case 'ncoords'
        % Return the number of coordinates in the current view. See case
        % 'coords'.
        %   nCoords = viewGet(vw, 'number of coords');
        val = size( viewGet(vw, 'Coords'), 2 );

    case 'classfilename'
        % Return the path to either the left or the right gray/white
        % classification file.
        %   fname = viewGet(vw, 'class file name', 'left');
        %   fname = viewGet(vw, 'class file name', 'right');
        if (length(varargin) == 1), hemisphere = varargin{1};
        else  hemisphere = 'left';
        end
        switch lower(hemisphere)
            case {'left' 'l'}
                if ~checkfields(vw,'leftClassFile') || isempty(vw.leftClassFile);
                    [~,val] = GetClassFile(vw, 0, 1);
                else
                    val = vw.leftClassFile;
                end
            case {'right' 'r'}
                if ~checkfields(vw,'rightClassFile') || isempty(vw.rightClassFile)
                    [~,val] = GetClassFile(vw, 1, 1);
                else
                    val = vw.rightClassFile;
                end
            otherwise
                error('Unknown hemisphere');
        end

    case {'classdata','class','classification','whitematter'}
        % classFileRight = viewGet(vw,'class data','right');
        if length(varargin) == 1, hemisphere = varargin{1};
        else error('You must specify right/left hemisphere.');
        end
        switch lower(hemisphere)
            case 'left'
                val = GetClassFile(vw, 0);
            case 'right'
                val = GetClassFile(vw, 1);
            otherwise
                error('Unknown hemisphere');
        end

    case {'graymatterfilename','graypath','grayfilename','grayfile'}
        % grayFile = viewGet(vw,'Gray matter filename','right');
        if length(varargin) == 1, hemisphere = varargin{1};
        else error('You must specify right/left hemisphere.');
        end
        switch lower(hemisphere)
            case 'left'
                if checkfields(vw,'leftPath')
                    val = vw.leftPath;
                end
            case 'right'
                if checkfields(vw,'rightPath')
                    val = vw.rightPath;
                end
            otherwise
                error('Unknown hemisphere');
        end

    otherwise
        error('Unknown viewGet parameter');

end

return


function hFit = dfaddparamfit(hFit, fitname, distname, dsname, fitframe, exclname, useestimated, fixedvals)
%DFADDPARAMFIT Add parametric fit in dfittool

%   $Revision: 1.1.6.10 $  $Date: 2004/01/24 09:35:10 $
%   Copyright 2003-2004 The MathWorks, Inc.

badfit = false;    % badfit=true means fit failed or not attempted
covok = true;      % covariance calculation can be done
if isempty(hFit)
    newfit = true;
    hFit = stats.dffit(fitname, fitframe);
else
    newfit = false;
end
listeners = hFit.listeners;
set(listeners, 'Enabled', 'off');

frameContents = fitframe.getContentPane;
componentsVector = get(frameContents,'Components');
fittingPanel = componentsVector(1);

% Get data set to fit
ds=find(getdsdb,'name',dsname);
hFit.distname = distname;
hFit.dataset  = dsname;
hFit.fittype = 'param';
hFit.dshandle = ds;

% Store some GUI values in fit
hFit.pfixedtext = fixedvals;
hFit.pestimated = useestimated;

% Extract data from this data set
alpha = 0.05;
hExcl = dfgetexclusionrule(exclname);
[x, cens, freq] = getincludeddata(ds,hExcl);

% Get information about the requested distribution
dist = dfgetdistributions(distname);
if length(dist)~=1 || isempty(x)
   if length(dist)~=1
      emsg = 'Bad distribution name.';
   else
      emsg = 'No data remaining after exclusion rule applied.';
   end
   wmsg = '';
   badfit = true;
end
if length(dist)==1
   hFit.enablebounds = dist.hasconfbounds;
end

% Perform the fit
lasterr('');
lastwarn('');
ws = warning('off');
if badfit
   p = [];
else
   try
      nparams = length(dist.pnames);
      if dist.censoring
         censargs = {cens freq};
      else
         if ~isempty(cens) && any(cens)
            error('stats:dfaddparamfit:NoCensoring',...
                  'Censoring not allowed with the %s distribution', distname);
         elseif ~isempty(freq) && any(freq~=1)
            x = expandInput(x,freq);
            freq = [];
         end
         censargs = {};
      end

      % How many output variables will this return?
      if dist.paramvec
         nparamvars = 1;
      else
         nparamvars = nparams;
      end

      fixedparams = cell(0,1);
      if any(~useestimated)
         for j=1:nparams
            if ~useestimated(j)
               txt = deblank(fixedvals{j});
               if isempty(txt)
                  error('stats:dfaddparamfit:BadParam',...
                        'Invalid value for parameter %s', dist.pnames{j});
               end
               num = str2double(txt);
               if ~isfinite(num)
                  error('stats:dfaddparamfit:BadParam',...
                        'Invalid value for parameter %s', dist.pnames{j});
               end
               fixedparams{length(fixedparams)+1} = num;
            end
         end
      end

      % Set up a cell array to receive outputs, then do the fit
      pcell = cell(nparamvars,1);
      [pcell{:}] = feval(dist.fitfunc, x, fixedparams{:}, alpha, censargs{:});

      % Extract results into a single vector
      if dist.paramvec
         p = pcell{1};
      else
         p = [pcell{:}];
      end
   catch
      p = [];
   end
end
warning(ws);

if ~badfit
   if ~isempty(lastwarn)
      wmsg = sprintf('Warning:  %s',lastwarn);
   else
      wmsg = '';
   end
   emsg = lasterr;
   newmsg = '';
   if any(~isfinite(p))
      newmsg = 'Fit produced infinite parameter estimates.';
   elseif numel(p)~=numel(dist.pnames) || ~isnumeric(p)
      newmsg = 'Fit function returned bad parameter values';
   end
   if ~isempty(newmsg)
      badfit = true;
      emsg = combinemsg(emsg,newmsg);
   end

   % Any type of failure so far makes the covariance calculation questionable
   if ~isempty(wmsg) || ~isempty(emsg)
      covok = false;
   end
end

% Try to get a likelihood value
if isempty(p)
   pcov = [];
   nlogl = NaN;
else
   try
      if ~isempty(dist.likefunc)
         if covok
            [nlogl,pcov] = feval(dist.likefunc, p, x, censargs{:});
         else
            nlogl = feval(dist.likefunc, p, x, censargs{:});
            pcov = [];
         end
      else
         pcov = [];
         nlogl = localnlogl(num2cell(p),dist.pdffunc,dist.cdffunc,x,cens,freq);
      end
      newmsg = '';
   catch
      newmsg = lasterr;
   end
   if isempty(newmsg) && (~isnumeric(nlogl) || ~isscalar(nlogl))
      newmsg = 'Result must be a numeric scalar';
   end
   if isnan(nlogl)
      nlogl = NaN;      % explicitly set to real nan to remove imaginary part
   end
   if ~isempty(newmsg);
      pcov = [];
      nlogl = NaN;
      wmsg = combinemsg(wmsg,...
                        sprintf('Error while evaluating likelihood:\n%s',...
                                newmsg));
   end

end

% Get the range over which to show the fit
dffig = dfgetset('dffig');
ax = findall(dffig,'Type','axes','Tag','main');
xlim = get(ax,'XLim');

% Create a fit object using the information we calculated
if badfit
   resultsText = emsg;
else
   try
      hFit = storefitresults(hFit, dist, p, pcov, nlogl, xlim, hExcl, exclname);
      resultsText = getresults(hFit);
   catch
      resultsText = lasterr;
      badfit = true;
   end
end

resultsText = combinemsg(wmsg,resultsText);

% Show results
hFit.resultstext = resultsText;
fittingPanel.setResults(resultsText)

if ~isempty(hFit)
   if ~newfit && ~(hFit.isgood == ~badfit)
   		com.mathworks.toolbox.stats.FitsManager.getFitsManager.fitIsGoodChanged(java(hFit), ~badfit);
   end
   hFit.isgood = ~badfit;
   if newfit
	  hFit.plot = 1;
      % Add to fit array
      connect(hFit,getfitdb,'up');
   end
end

if hFit.plot
   % Determine if bounds can be shown
   if ~dist.hasconfbounds
      hFit.showbounds = false;
   end

   % Update plotted curve
   updateplot(hFit);

   % Update plot limits
   dfswitchyard('dfupdatexlim');
   dfswitchyard('dfupdateylim');
end

set(listeners, 'Enabled', 'on');

if ~newfit
   com.mathworks.toolbox.stats.FitsManager.getFitsManager.fitChanged(...
       java(hFit),fitname,fitname);
end

% Display a more prominent warning outside the results text
if ~badfit && ~isempty(wmsg)
   warndlg(wmsg,'Distribution Fitting Warning','modal');
end

% ----------------------------------------------
function hFit = storefitresults(hFit, dist, p, pcov, nlogl, xlim, hExcl, exclname)
% Update its properties
hFit.distspec = dist;
hFit.params   = p;
hFit.pcov     = pcov;
hFit.pfixed   = false(size(p));
hFit.loglik = -nlogl;
hFit.support = dist.support;
hFit.exclusionrule = hExcl;
hFit.exclusionrulename = exclname;

hFit.xlim = xlim;
setftype(hFit,dfgetset('ftype'));

% ---------------------------------------------
function nlogl = localnlogl(p, pdf, cdf, x, cens, freq)
% Calculate negative log likelihood

% Handle defaults for option inputs
if isempty(cens)
   cens = false(size(x));
else
   cens = (cens == 1);
end
if isempty(freq)
   freq = ones(size(x));
end

% Compute for uncensored observations
nlogl = - sum(freq(~cens) .* log(feval(pdf, x(~cens), p{:})));

% Add component for censored observations
if any(cens)
   nlogl = nlogl - sum(freq(cens) .* log(1-feval(cdf, x(cens), p{:})));
end

% -----------------------------------
function msg = combinemsg(msg,newmsg)
%COMBINEMSG Combine multiple messages into a single message
if isempty(msg)
   msg = newmsg;
elseif ~isempty(newmsg)
   msg = sprintf('%s\n\n%s',msg,newmsg);
end

% -----------------------------------------
function expanded = expandInput(input,freq)
%EXPANDDATA Expand out an input vector using element frequencies.
if ~isequal(size(input),size(freq))
    error('stats:dfaddparamfit:InputSizeMismatch',...
          'Input argument sizes must match.');
end

% Remove points that have zero frequency
t = (freq == 0);
if any(t)
   input(t) = [];
   freq(t) = [];
end

% Expand the remainder
i = cumsum(freq);
j = zeros(1, i(end));
j(i(1:end-1)+1) = 1;
j(1) = 1;
expanded = input(cumsum(j));


%Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
%Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
%
%THE BSD LICENSE
%
%Redistribution and use in source and binary forms, with or without
%modification, are permitted provided that the following conditions
%are met:
%
%1. Redistributions of source code must retain the above copyright
%   notice, this list of conditions and the following disclaimer.
%2. 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.
%
%THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 THE AUTHOR 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.

function flann_set_distance_type(type, order)
%FLANN_LOAD_INDEX  Loads an index from disk
%
% Marius Muja, March 2009

    distances = struct('euclidean', 1, 'manhattan', 2, 'minkowski', 3, 'max_dist', 4, 'hik', 5, 'hellinger', 6, 'chi_square', 7, 'cs', 7, 'kullback_leibler', 8, 'kl', 8);
    function id = value2id(map,value)
        id = map.(value);
    end

    if ~isnumeric(type),
        type = value2id(distances,type);
    end
    if type~=3
        order = 0;
    end
    nearest_neighbors('set_distance_type', type, order);
end


function write_neuralynx_ncs(filename, ncs);

% WRITE_NEURALYNX_NCS writes continuous data to a NCS file
% The input data should be scaled in uV.
%
% Use as
%   write_neuralynx_ncs(filename, ncs)
%
% See also READ_NEURALYNX_NCS

% Copyright (C) 2005-2007, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id: write_neuralynx_ncs.m 2885 2011-02-16 09:41:58Z roboos $

if ~isa(ncs.TimeStamp, 'uint64')
  error('timestamps should be uint64');
end

% convert the data from uV into V
ncs.dat = ncs.dat * 1e-6;
% scale the data and convert to 16 bits,
% this has to be done prior to writing the header
ADMaxValue = double(intmax('int16'));
ADMaxVolts = max(abs(ncs.dat(:)));
if ADMaxVolts>0
  ADBitVolts = ADMaxVolts / ADMaxValue;
else
  ADBitVolts = 1;
end
ncs.dat = int16(ncs.dat / ADBitVolts);
% update the header with the calibration values
ncs.hdr.ADBitVolts = ADBitVolts;
ncs.hdr.ADMaxValue = ADMaxValue;

% construct the header
buf  = [];
buf  = [buf sprintf('######## Neuralynx Data File Header\r\n')];
buf  = [buf sprintf('## File Name: %s\r\n', filename)];
buf  = [buf sprintf('## Time Opened: (m/d/y): %s  At Time: %s\r\n', datestr(clock, 'mm/dd/yy'), datestr(clock, 'HH:MM:SS'))];
f = fieldnames(ncs.hdr);
for i=1:length(f)
  v = getfield(ncs.hdr, f{i});
  switch class(v)
    case 'char'
      buf = [buf sprintf('-%s\t%s\r\n', f{i}, v)];
    case 'double'
      buf = [buf sprintf('-%s\t%s\r\n', f{i}, num2str(v))];
    otherwise
      error('unknown class in writing header');
  end
end

% pad the rest of the header with zeros
buf((end+1):16384) = 0;

% open the file and write the header
fid  = fopen(filename, 'wb', 'ieee-le');
fwrite(fid, buf);

% The format of a continuous sampled record is
%   int64 TimeStamp
%   int32 ChanNumber
%   int32 SampFreq
%   int32 NumValidSamp
%   int16 Samp[0] ... int16 Samp[511]
% Note that if NumValidSamp < 512, Samp[n], where n >= NumValidSamp, will
% contain random data.

for i=1:size(ncs.dat,2)
  % write a single continuous data record
  fwrite(fid, ncs.TimeStamp(i)   , 'uint64');
  fwrite(fid, ncs.ChanNumber(i)  , 'int32');
  fwrite(fid, ncs.SampFreq(i)    , 'int32');
  fwrite(fid, ncs.NumValidSamp(i), 'int32');
  fwrite(fid, ncs.dat(:,i)       , 'int16');
end

% close the file
fclose(fid);


classdef tblock < matlab.unittest.TestCase
    % tblock   Unit tests for transformer.layer.block

    % Copyright 2020 The MathWorks, Inc.

    properties(Constant, Access=private)
        block = @gpt2.layer.block
    end

    properties(TestParameter)
        Input = struct(...
            'Scalar', 1,...
            'Vector', 1:5,...
            'Matrix', reshape(1:6,[3,2]))
    end

    methods(Test)
        function outputHasInputSize(test,Input)
            % The block is simply a composition of other layers. Simply
            % verify the output of a block is the same size as the input,
            % and as such the blocks can be stacked.
            x = dlarray(Input);
            C = size(Input,1);
            weights = test.randomWeights(C);
            hyperParameters.NumHeads = 1;
            y = test.block(x,[],weights,hyperParameters);
            test.verifySize(y,size(x));
        end

        function outputHasInputSizeWithPasts(test,Input)
            % As above but using "pasts" - a concatenation of key and value
            % matrices.
            x = dlarray(Input);
            C = size(Input,1);
            weights = test.randomWeights(C);
            hyperParameters.NumHeads = 1;
            % Provide a fake past of sequence length 1
            K_fake = dlarray(rand(C,1));
            V_fake = dlarray(rand(C,1));
            past = cat(5,K_fake,V_fake);
            [y,present] = test.block(x,past,weights,hyperParameters);
            test.verifySize(y,size(x));
            % The size of presents is the size of past except the sequence
            % dimension gets extended by the sequence length of y
            exp_present_size = size(past);
            exp_present_size(2) = exp_present_size(2)+size(y,2);
            test.verifySize(present,exp_present_size);
        end
    end

    methods(Access=private)
        function weights = randomWeights(test,C)
            % C is num features, or latent dimension of the block
            g1 = dlarray(rand(C,1));
            b1 = dlarray(rand(C,1));
            g2 = dlarray(rand(C,1));
            b2 = dlarray(rand(C,1));
            W_A1 = dlarray(rand(3*C,C));
            W_A2 = dlarray(rand(C));
            b_A1 = dlarray(rand(3*C,1));
            b_A2 = dlarray(rand(C,1));
            W_P1 = dlarray(rand(C));
            b_P1 = dlarray(rand(C,1));
            W_P2 = dlarray(rand(C));
            b_P2 = dlarray(rand(C,1));
            weights = test.prepareBlockWeightsStruct(g1,b1,W_A1,b_A1,W_A2,b_A2,g2,b2,W_P1,b_P1,W_P2,b_P2);
        end

        function s = prepareBlockWeightsStruct(test,g1,b1,W_A1,b_A1,W_A2,b_A2,g2,b2,W_P1,b_P1,W_P2,b_P2)
            % Merge various structs that have the appropriate weight naming
            % syntax.
            s_ln = test.prepareLayerNormWeightsStruct(g1,b1,g2,b2);
            s_attn = test.prepareAttentionWeightsStruct(W_A1,b_A1,W_A2,b_A2);
            s_mlp = test.prepareMLPWeightsStruct(W_P1,b_P1,W_P2,b_P2);
            c = {s_ln,s_attn,s_mlp};
            fn = cellfun(@fieldnames,c,'UniformOutput',false);
            fn = cat(1,fn{:});
            fv = cellfun(@struct2cell,c,'UniformOutput',false);
            fv = cat(1,fv{:});
            s = struct();
            for i = 1:numel(fn)
                s.(fn{i}) = fv{i};
            end
        end

        function s = prepareAttentionWeightsStruct(~,W1,b1,W2,b2)
            % Prepare a struct compatible with the weights input of
            % attention. These are for the fully connected layers.
            s = struct(...
                'attn_c_attn_w_0',W1,...
                'attn_c_attn_b_0',b1,...
                'attn_c_proj_w_0',W2,...
                'attn_c_proj_b_0',b2);
        end

        function s = prepareLayerNormWeightsStruct(~,g1,b1,g2,b2)
            % Prepare a struct of weights compatible with the two layer
            % norm calls in block
            s = struct(...
                'ln_1_g_0',g1,...
                'ln_1_b_0',b1,...
                'ln_2_g_0',g2,...
                'ln_2_b_0',b2);
        end

        function s = prepareMLPWeightsStruct(~,W1,b1,W2,b2)
            % Create a struct of weights to be consumed by
            % transformer.layer.multiLayerPerceptron
            s = struct(...
                'mlp_c_fc_w_0',W1,...
                'mlp_c_fc_b_0',b1,...
                'mlp_c_proj_w_0',W2,...
                'mlp_c_proj_b_0',b2);
        end
    end
end


function val = fgGet(fg,param,varargin)
%Get values from a fiber group structure
%
%  val = fgGet(fg,param,varargin)
%
% Parameters
% General
%    'name'
%    'type'
%    'colorrgb'
%    'thickness'
%    'visible'
%
% Fiber related
%    'nfibers'- Number of fibers in this group
%    'nodes per fiber'  - Number of nodes per fiber.
%    'fibers' - Fiber coordinates
%    'fibernames'
%    'fiberindex'
%
% ROI and image coord related
%    'unique image coords'
%    'nodes to imagecoords' -
%    'voxel2fiber node pairs' - For each roi coord, an Nx2 matrix of
%        (fiber number,node number)
%    'nodes in voxels' - Nodes inside the voxels of roi coords
%    'voxels in fg'  - Cell array of the roiCoords touched by each fiber
%    'voxels2fibermatrix' - Binary matrix (voxels by fibers).  1s when a
%        fiber is in a voxel of the roiCoords (which are, sadly, implicit).
%
% Tensor and tractography related
%      'tensors'     - Tensors for each node
%
%
% See also: dwiGet/Set, fgCreate; fgSet
%
% (c) Stanford VISTA Team

% NOTES:
% Programming TODO:
%   We should store the transforms needed to shift the fg coordinates
%   between acpc and image space.

% I eliminated these checks because this function is now called many many
% times and this slows the computations (Franco).
%
%if notDefined('fg'),error('fiber group required.'); end
% if notDefined('param'), error('param required.'); end

val = [];

switch mrvParamFormat(param)

  % Basic fiber parameters
  case 'name'
    val = fg.name;
  case 'type'  % Should always be fibergroup
    val = fg.type;

    % Fiber visualization settings.
  case 'colorrgb'
    val = fg.colorRgb;
  case 'thickness'
    val = fg.thickness;
  case 'visible'
    val = fg.visible;

    % Simple fiber properties --
  case {'fibers'}
    % val = fgGet(fg,'fibers',fList);
    %
    % Returns a 3xN matrix of fiber coordinates corresponding to the
    % fibers specified in the integer vector, fList.  This differs from
    % the dtiH (mrDiffusion) representation, where fiber coordinates
    % are stored as a set of cell arrays for each fiber.
    if ~isempty(varargin)
      list = varargin{1};
      val = cell(length(list),1);
      for ii=1:length(list)
        val{ii} = fg.fibers{ii};
      end
    else
      val = fg.fibers;
    end
  case 'fibernames'
    val = fg.fiberNames;
  case 'fiberindex'
    val = fg.fiberIndex;
  case 'nfibers'
    val = length(fg.fibers);
  case {'nodesperfiber','nsamplesperfiber','nfibersamples'}
    % fgGet(fg,'n samples per fiber ')
    % How many samples per fiber.  This is about equal to
    % their length in mm, though we need to write the fiber lengths
    % routine to actually calculate this.
    nFibers = fgGet(fg,'n fibers');
    val = zeros(1,nFibers);
    for ii=1:nFibers
      val(ii) = length(fg.fibers{ii});
    end

    % Fiber group (subgroup) properties.
    % These are used when we classify fibers into subgroups.  We should
    % probably clean up this organization which is currently
    %
    %   subgroup - length of fibers, an index of group identity
    %   subgroupNames()
    %    .subgroupIndex - Probably should go away and the index should
    %          just be
    %    .subgroupName  - Probably should be moved up.
    %
  case {'ngroups','nsubgroups'}
    val = length(fg.subgroupNames);
  case {'groupnames'}
    val = cell(1,fgGet(fg,'n groups'));
    for ii=1:nGroups
      val{ii} = fg.subgroupNames(ii).subgroupName;
    end

    % DTI properties
  case 'tensors'
    val = fg.tensors;

    % Fiber to coord calculations
  case {'imagecoords'}
    % c = fgGet(fgAcpc,'image coords',fgList,xForm);
    % c = fgGet(fgAcpc,'image coords',fgList,xForm);
    %
    % Return the image coordinates of a specified list of fibers
    % Returns a matrix that is fgList by 3 of the image coordinates for
    % each node of each fiber.
    %
    % Fiber coords are represented at fine resolution in ACPC space.
    % These coordinates are rounded and in image space
    if ~isempty(varargin)
      fList = varargin{1};
      if length(varargin) > 1
        xForm = varargin{2};
        % Put the fiber coordinates into image space
        fg = dtiXformFiberCoords(fg,xForm);
      end
    else
      % In this case, the fiber coords should already be in image
      % space.
      nFibers = fgGet(fg,'n fibers');
      fList = 1:nFibers;
    end

    % Pull out the coordinates and floor them.  These are in image
    % space.
    nFibers = length(fList);
    val = cell(1,nFibers);
    if nFibers == 1
      %val = round(fg.fibers{fList(1)}');
      val = floor(fg.fibers{fList(1)}');

    else
      for ii=1:nFibers
        %val{ii} = round(fg.fibers{fList(ii)}');
        val{ii} = floor(fg.fibers{fList(ii)}');

      end
    end

  case {'uniqueimagecoords'}
    %   coords = fgGet(fgIMG,'unique image coords');
    %
    % The fg input must be in IMG space.
    %
    % Returns the unique image coordinates of all the fibers as an Nx3
    % matrix of integers.
    % val = round(horzcat(fg.fibers{:})');
    val = floor(horzcat(fg.fibers{:})');
    val = unique(val,'rows');

  case {'nodes2voxels'}
    %   nodes2voxels = fgGet(fgImg,'nodes2voxels',roiCoords)
    %
    % The roiCoords are a matrix of Nx3 coordinates.  They describe a
    % region of interest, typically in image space or possibly in acpc
    % space.
    %
    % We return a cell array that is a mapping of fiber nodes to voxels in
    % the roi.  The roi is specified as an Nx3 matrix of coordinates.
    % The returned cell array, nodes2voxels, has the same number of
    % cells as there are fibers.
    %
    % Unlike the fiber group cells, which have a 3D coordinate of each
    % node, this cell array has an integer that indexes the row of
    % roiCoords that contains the node. If a node is not in any of the
    % roiCoords, the entry in node2voxels{ii} for that node is zero.
    % This means that node is outside the 'roiCoords'.
    %
    % Once again: The cell nodes2voxels{ii} specifies whether each
    % node in the iith fiber is inside a voxel in the roiCoords.  The
    % value specifies the row in roiCoords that contains the node.
    %
    if isempty(varargin), error('roiCoords required');
    else
      roiCoords = varargin{1};
    end

    % Find the roiCoord for each node in each fiber.
    nFiber = fgGet(fg,'n fibers');
    val    = cell(nFiber,1);
    for ii=1:nFiber
      % if ~mod(ii,200), fprintf('%d ',ii); end
      % Node coordinates in image space
      nodeCoords = fgGet(fg,'image coords',ii);

      % The values in loc are the row of the coords matrix that contains
      % that sample point in a fiber.  For example, if the number 100 is
      % in the 10th position of loc, then the 10th sample point in the
      % fiber passes through the voxel in row 100 of coords.
      [~, val{ii}] = ismember(nodeCoords, roiCoords, 'rows');
    end

  case {'voxel2fibernodepairs','v2fn'}
    % voxel2FNpairs = fgGet(fgImg,'voxel 2 fibernode pairs',roiCoords);
    % voxel2FNpairs = fgGet(fgImg,'voxel 2 fibernode pairs',roiCoords,nodes2voxels);
    %
    % The return is a cell array whose size is the number of voxels.
    % The cell is a Nx2 matrix of the (fiber, node) pairs that pass
    % through it.
    %
    % The value N is the number of nodes in the voxel.  The first
    % column is the fiber number.  The second column reports the indexes
    % of the nodes for each fiber in each voxel.
    tic
    fprintf('\n[fgGet] Computing fibers/nodes pairing in each voxel...')
    if length(varargin) < 1, error('Requires the roiCoords.');
    else
      roiCoords = varargin{1};
      nCoords = size(roiCoords,1);
    end
    if length(varargin) < 2
      % We assume the fg and the ROI coordinates are in the same
      % coordinate frame.
      nodes2voxels    = fgGet(fg,'nodes 2 voxels',roiCoords);
    else nodes2voxels = varargin{2};
    end

    nFibers      = fgGet(fg,'nFibers');
    voxelsInFG   = fgGet(fg,'voxels in fg',nodes2voxels);
    roiNodesInFG = fgGet(fg,'nodes in voxels',nodes2voxels);
    val = cell(1,nCoords);
    for thisFiber=1:nFibers
      voxelsInFiber = voxelsInFG{thisFiber};   % A few voxels, in a list
      nodesInFiber  = roiNodesInFG{thisFiber}; % The corresponding nodes

      % Then add a row for each (fiber,node) pairs that pass through
      % the voxels for this fiber.
      for jj=1:length(voxelsInFiber)
        thisVoxel = voxelsInFiber(jj);
        % Print out roi coord and fiber coord to verify match
        % roiCoords(thisVoxel,:)
        % fg.fibers{thisFiber}(:,nodesInFiber(jj))
        % Would horzcat be faster?
        val{thisVoxel} = cat(1,val{thisVoxel},[thisFiber,nodesInFiber(jj)]);
      end
    end
    fprintf('process completed in: %2.3fs.\n',toc)

  case {'nodesinvoxels'}
    % nodesInVoxels = fgGet(fg,'nodes in voxels',nodes2voxels);
    %
    % This cell array is a modified form of nodes2voxels (see above).
    % In that cell array every node in every fiber has a number
    % referring to its row in roiCoords, or a 0 when the node is not in
    % any roiCoord voxel.
    %
    % This cell array differs only in that the 0s removed.  This
    % is used to simplify certain calculations.
    %
    if length(varargin) <1
      error('Requires nodes2voxels cell array.');
    end

    nodes2voxels = varargin{1};
    nFibers = fgGet(fg,'nFibers');
    val = cell(1,nFibers);

    % For each fiber, this is a list of the nodes that pass through
    % a voxel in the roiCoords
    for ii = 1:nFibers
      % For each fiber, this is a list of the nodes that pass through
      % a voxel in the roiCoords
      lst = (nodes2voxels{ii} ~= 0);
      val{ii} = find(lst);
    end

  case 'voxelsinfg'
    % voxelsInFG = fgGet(fgImg,'voxels in fg',nodes2voxels);
    %
    % A cell array length n-fibers. Each cell has a list of the voxels
    % (rows of roiCoords) for a fiber.
    %
    % This routine eliminates the 0's in the nodes2voxels lists.
    %
    if length(varargin) < 1, error('Requires nodes2voxels cell array.'); end

    nodes2voxels = varargin{1};
    nFibers = fgGet(fg,'nFibers');
    val = cell(1,nFibers);
    for ii = 1:nFibers
      % These are the nodes that pass through a voxel in the
      % roiCoords
      lst = (nodes2voxels{ii} ~= 0);
      val{ii} = nodes2voxels{ii}(lst);
    end

  case {'voxels2fibermatrix','v2fm'}
    %   v2fm = fgGet(fgImg,'voxels 2 fiber matrix',roiCoords);
    % Or,
    %   v2fnPairs = fgGet(fgImg,'v2fn',roiCoords);
    %   v2fm = fgGet(fgImg,'voxels 2 fiber matrix',roiCoords, v2fnPairs);
    %
    % mrvNewGraphWin; imagesc(v2fm)
    %
    % Returns a binary matrix of size Voxels by Fibers.
    % When voxel ii has at least one node from fiber jj, there is a one
    % in v2fm(ii,jj).  Otherwise, the entry is zero.
    %

    % Check that the fg is in the image coordspace:
    if isfield(fg, 'coordspace') && ~strcmp(fg.coordspace, 'img')
      error('Fiber group is not in the image coordspace, please xform');
    end

    if isempty(varargin), error('roiCoords required');
    else
      roiCoords = varargin{1};
      nCoords   = size(roiCoords,1);
      if length(varargin) < 2
        v2fnPairs = fgGet(fg,'v2fn',roiCoords);
      else
        v2fnPairs = varargin{2};
      end
    end

    % Allocate matrix of voxels by fibers
    val = zeros(nCoords,fgGet(fg,'n fibers'));

    % For each coordinate, find the fibers.  Set those entries to 1.
    for ii=1:nCoords
      if ~isempty(v2fnPairs{ii})
        f = unique(v2fnPairs{ii}(:,1));
      end
      val(ii,f) = 1;
    end

  case {'fibersinroi','fginvoxels','fibersinvoxels'}
    % fList = fgGet(fgImg,'fibersinroi',roiCoords);
    %
    % v2fn = fgGet(fgImg,'v2fn',roiCoords);
    % fList = fgGet(fgImg,'fibersinroi',roiCoords,v2fn);
    %
    % Returns an integer vector of the fibers with at least
    % one node in a region of interest.
    %
    % The fg and roiCoords should be in the same coordinate frame.
    %
    if isempty(varargin), error('roiCoords required');
    elseif length(varargin) == 1
      roiCoords = varargin{1};
      v2fnPairs = fgGet(fg,'v2fn',roiCoords);
    elseif length(varargin) > 1
      roiCoords = varargin{1};
      v2fnPairs = varargin{2};
    end

    val = []; nCoords = size(roiCoords,1);
    for ii=1:nCoords
      if ~isempty(v2fnPairs{ii})
        val = cat(1,val,v2fnPairs{ii}(:,1));
      end
    end
    val = sort(unique(val),'ascend');

  case {'coordspace','fibercoordinatespace','fcspace'}
    % In some cases, the fg might contain information telling us in which
    % coordinate space its coordinates are set. This information is set
    % as a struct. Each entry in the struct can be either a 4x4 xform
    % matrix from the fiber coordinates to that space (with eye(4) for
    % the space in which the coordinates are defined), or (if the xform
    % is not know) an empty matrix.

    cspace_fields = fields(fg.coordspace);
    val = [];
    for f=1:length(cspace_fields)
      this_field = cspace_fields{f};
      if isequal(getfield(fg.coordspace, this_field), eye(4))
        val = this_field;
      end
    end

  otherwise
    error('Unknown fg parameter: "%s"\n',param);
end

return


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

if nargin<2
    database = 'pascal2012';
end
if nargin<3
    gt_set = 'val2012';
end
if nargin<4
    % Number of sampled candidates
    n_cands = [10:5:100,125:25:1000,1500:500:6000,10000];
end

% Get the name of the folder to refer to the method
if strcmp(labels_folder(end),filesep)
    labels_folder(end) = [];
end
tmp = strfind(labels_folder,filesep);
if isempty(tmp)
    method_name = labels_folder;
else
    method_name = labels_folder(tmp(end)+1:end);
end

res_dir    = fullfile(root_dir,'results',database, method_name);
stats_file = fullfile(root_dir,'results',database, [method_name '_' gt_set '.mat']);

% Is the result already gathered?
if exist(stats_file, 'file')
    load(stats_file)
    recompute = 0;
    disp(['Loaded: ' stats_file '.'])
else
    disp(['RECOMPUTING: ' stats_file '.'])
    recompute = 1;
end

if recompute
    %% Evaluate and save each image independently to be able to parallelize
    % You can adapt the matlabpool to your system
    eval_and_save_labels(labels_folder,database,gt_set);

    %% Gather and save results
    % Load which images to consider
    im_ids = database_ids(database,gt_set);

    % Store and initialize
    stats.n_cands = n_cands;
    stats.gt_set  = gt_set;
    stats.obj_classes = [];

    % Compute statistics
    stats.num_objects = 0;
    for ii=1:numel(im_ids)
        res_file = fullfile(res_dir, [im_ids{ii} '.mat']);
        if exist(res_file,'file')
            load(res_file)
            for jj=1:length(stats.n_cands)
                curr_n_cands = min(stats.n_cands(jj), size(jaccards,2));
                to_consider = 1:curr_n_cands;
                stats.all_n_masks(ii,jj) = length(to_consider);
                if (stats.all_n_masks(ii,jj)>0)
                    for kk=1:size(jaccards,1)
                        [stats.max_J(stats.num_objects+kk,jj), which_one] = max(jaccards(kk,to_consider));
                        stats.max_indicator(stats.num_objects+kk,jj) = to_consider(which_one);
                        stats.max_fp(stats.num_objects+kk,jj) = false_pos(kk,to_consider(which_one));
                        stats.max_fn(stats.num_objects+kk,jj) = false_neg(kk,to_consider(which_one));
                        stats.max_inters(stats.num_objects+kk,jj) = inters(kk,to_consider(which_one));
                    end
                end
            end
            stats.obj_classes = [stats.obj_classes; obj_classes(1:size(inters,1))];
            stats.num_objects = stats.num_objects + size(jaccards,1);
        else
            error([res_file ' not found']);
        end
    end

    % Check
    if size(stats.obj_classes,1)~=stats.num_objects
        error('Something went wrong')
    end

    stats.mean_n_masks   = mean(stats.all_n_masks);

    % ----- Jaccard at instance level (J_i) ----
    % It is the mean best jaccard for all objects
    stats.jaccard_object = mean(stats.max_J);

    % ----- Compute jaccard at class level (J_c) ----
    class_ids = unique(stats.obj_classes);
    for ii=1:length(class_ids)
        curr_class = class_ids(ii);
        stats.per_class_results{ii}.num_objects = sum(stats.obj_classes==curr_class);

        stats.per_class_results{ii}.max_fp     = stats.max_fp(logical(stats.obj_classes==curr_class),:);
        stats.per_class_results{ii}.max_fn     = stats.max_fn(logical(stats.obj_classes==curr_class),:);
        stats.per_class_results{ii}.max_inters = stats.max_inters(logical(stats.obj_classes==curr_class),:);
        stats.per_class_results{ii}.max_J      = stats.max_J(logical(stats.obj_classes==curr_class),:);
        stats.per_class_results{ii}.meanmax    = mean(stats.per_class_results{ii}.max_J);

        stats.per_class_results{ii}.global_fp      = sum(stats.per_class_results{ii}.max_fp,1);
        stats.per_class_results{ii}.global_fn      = sum(stats.per_class_results{ii}.max_fn,1);
        stats.per_class_results{ii}.global_inters  = sum(stats.per_class_results{ii}.max_inters,1);

        % Compute per-class total inters, fp, fn
        stats.per_class_results{ii}.global_J = ...
             stats.per_class_results{ii}.global_inters ./...
            (stats.per_class_results{ii}.global_inters+...
             stats.per_class_results{ii}.global_fp    +...
             stats.per_class_results{ii}.global_fn);
    end

    % Compute global mean on all classes
    tmp = [];
    for ii=1:length(class_ids)
        tmp = [tmp; stats.per_class_results{ii}.global_J]; %#ok<AGROW>
    end
    stats.jaccard_class = mean(tmp,1);

    save(stats_file,'stats');

    % Remove temporal results folder
    rmdir(res_dir,'s');
end
end


function [sicd_meta] = rgazcomp_file(input_filename_cphd, output_filename_sicd, varargin)
%RGAZCOMP_FILE Applies the most simple of IFPs (range-azimuth compression)
%on a file of motion-compensated phase history data in CPHD file format and
%writes to a SICD file with appropriate metadata
%
% RGAZCOMP_FILE(INPUT_FILENAME_CPHD, OUTPUT_FILENAME_SICD, 'PropertyName', PropertyValue, ...)
%
%       Property name     Description
%       resolution        Desired resolution 3dB IPR width
%                         ([range_resolution azimuth_resolution]) in
%                         meters.  The necessary pulses and samples for
%                         image formation at this resolution will be
%                         calculated (taken from middle of data.) If
%                         pulse_range and/or sample_range properties are
%                         defined, then this parameter is ignored.  Default
%                         is the full resolution that the collect supports.
%       pulse_range       Set of pulses to use for image formation. (For
%                         example, 1:1000.) Default is to calculate this
%                         from resolution property.
%       sample_range      Set of samples to use for image formation. (For
%                         example, 1:1000.) Default is to calculate this
%                         from resolution property.
%       channel           Channel to use from CPHD file.  Default is 1.
%       sample_rate       Image domain oversample ratios.  Default is 1.
%       max_block_size    When data can't be processed within memory, this
%                         is the size of the blocks (in bytes) to use
%                         process the data.
%       quiet             If false, this reports stats on collection and
%                         IFP parameters.  Default is true.
%
% Note for larger datasets, this routine executes "out of core" storing of
% intermediate results in temporary files, which should be automatically
% cleaned up upon completion of this function.  However, if function is
% stopped in the middle, either through an error or user intervention, some
% of the files may remain in MATLAB's temporary directory (tempdir).
%
% Authors: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Parse input parameters and read metadata
ph_reader = open_ph_reader(input_filename_cphd);
ifp_params = select_pulses_samples_cphd(ph_reader, varargin{:}); % Parse generic IFP parameters
p1 = inputParser;
p1.KeepUnmatched=true;
p1.addParamValue('max_block_size',[], @isscalar);
p1.addParamValue('sample_rate',1, @isscalar);
p1.FunctionName = mfilename;
p1.parse(varargin{:});
ifp_params.sample_rate = p1.Results.sample_rate;
max_block_size = p1.Results.max_block_size;
if isempty(max_block_size)
    if ispc
        [uv, sv] = memory;
        % max_block_size is just the size of the largest single array we
        % will handle at once.  We will need significantly more memory than
        % this.  A factor of 16 seems to be reasonable.  Its a nice round
        % power of two that seems to avoid swapping in limited testing.
        max_block_size = min(sv.PhysicalMemory.Available/16, ...
            uv.MaxPossibleArrayBytes);
    else % Can't gauge memory in UNIX, so just put arbitrary value
        max_block_size = 2^27; % About 100Meg
    end
end
cphd_meta = ph_reader.get_meta();
[ignore, nbdata] = ph_reader.read_cphd(ifp_params.pulse_range, [], 1);
if ~strcmpi(cphd_meta.CollectionID.RadarMode.ModeType,'SPOTLIGHT') || ...
        any(any(diff(nbdata.SRPPos)))  % Assure spotlight data
    error('RGAZCOMP_FILE:UNSUPPORTED_COLLECT_TYPE','Unsupported collection mode.  Only spotlight data is supported.');
end
if any(diff(nbdata.SC0)) || any(diff(nbdata.SCSS))
    error('RGAZCOMP_FILE:UNSUPPORTED_COLLECT_TYPE','Unsupported data type.  Must have constant Fx0/Fx_SS.');
end
if any(diff(diff(ifp_params.sample_range)))
    error('RGAZCOMP_FILE:UNSUPPORTED_COLLECT_TYPE','Selected samples must be regularly spaced.');
end
if ~strcmp(cphd_meta.Global.DomainType, 'FX')
    error('RGAZCOMP_FILE:UNSUPPORTED_FILE_TYPE','Unsupported CPHD type.  Currently only FX domain is supported.');
end

%% Compute some values we will need
% Number of pulses and samples that we will be processing in this function.
% Defined here purely for conciseness of notation.
num_pulses  = length(ifp_params.pulse_range);
num_samples = length(ifp_params.sample_range);

% Decide whether we can process in memory, or whether we need to use
% intermediate files.  We hope to avoid using intermediate files since file
% reads and writes take the great majority of the processing time here.  In
% one profiling test, file reads and writes took 98% of the time of this
% funcion.  For this reason, we have attempted to minimize the number of
% reads and writes, and we try to comment in the code sections that affect
% these file I/O delays.  Unfortunately virtual memory and swapping (which
% is non-optimal file I/O out of our control) is much worse than the file
% I/O explicitly handled in this function, so try to set max_block_size to
% something as large as possible without the operating system resorting to
% virtual memory.
data_element_size = 8; % In bytes.  Assumes single precision data
process_in_blocks = max_block_size<(num_pulses*num_samples*prod(ifp_params.sample_rate)*data_element_size);
if process_in_blocks
    warning('RGAZCOMP_FILE:FILE_PROCESSING','This data must be processed through intermediate files, rather than all in memory.  This could take a while...');
end

ifp_params.image_size_pixels = floor(ifp_params.sample_rate .* [num_samples num_pulses]);
sicd_meta = rgazcomp_sicd_meta(cphd_meta,nbdata,ifp_params);

%% Range FFT across samples
% The FFT will be done in separably in two steps, first in range and then
% in azimuth.

% Setup temporary file
% The range FFT stage includes an in-order read (CPHD is written in
% "pulse-major" order) and an in-order write (data interpolated in range).
% We have to "corner-turn" (or transpose) after the range FFT (as we do on
% all 4 steps), but it is typically faster to read from a file out-of-order
% than write out-of-order, so we will do the corner-turn in the read of the
% azimuth FFT, rather than the write of this range FFT stage.
if process_in_blocks
    filename_range_fft = tempname;
    tempsicdmeta.ImageData.NumRows   = num_pulses;
    tempsicdmeta.ImageData.NumCols   = ifp_params.image_size_pixels(1);
    writer_range_interp = SIOWriter(filename_range_fft, tempsicdmeta, 'include_sicd_metadata', false);
end

% Iterate through sets of pulses for range FFT
first_pulse_in_set=1; % Out of all pulses selected for processing
max_num_lines = floor(max_block_size/(num_samples*data_element_size));
wb_hand=waitbar(0,'Range FFT...');
while(first_pulse_in_set<num_pulses)
    waitbar((first_pulse_in_set-1)/num_pulses,wb_hand);
    pulse_indices = first_pulse_in_set:...
        min(num_pulses,first_pulse_in_set+max_num_lines-1);
    data_block = ph_reader.read_cphd(ifp_params.pulse_range(pulse_indices),...
        ifp_params.sample_range, ifp_params.channel);

    data_block = pfa_fft_zeropad_1d(data_block,ifp_params.sample_rate);

    if process_in_blocks
        writer_range_interp.write_chip(data_block,[1 first_pulse_in_set]); % Save range interpolated data in this block
    end
    first_pulse_in_set=pulse_indices(end)+1;
end
if process_in_blocks
    clear writer_range_interp % Close file
end
ph_reader.close();

%% Azimuth FFT across vectors
if process_in_blocks
    reader_range_fft = open_reader(filename_range_fft);
end
writer_final_sicd = SICDWriter(output_filename_sicd, sicd_meta); % Final image will be written here

% Iterate through blocks of data for azimuth FFT
first_line_in_block=1; % Out of all rows in the final image
max_num_lines = floor(max_block_size/(num_pulses*data_element_size));
waitbar(0,wb_hand,'Azimuth FFT...');
while(first_line_in_block<ifp_params.image_size_pixels(1))
    waitbar((first_line_in_block-1)/ifp_params.image_size_pixels(1),wb_hand);
    line_range = [first_line_in_block ...
        min(ifp_params.image_size_pixels(1),first_line_in_block+max_num_lines-1)];
    if process_in_blocks
        data_block = reader_range_fft.read_chip(line_range, [1 num_pulses]);
    end

    data_block = pfa_fft_zeropad_1d(data_block.',ifp_params.sample_rate);
    if isfield(sicd_meta,'SCPCOA') && isfield(sicd_meta.SCPCOA,'SideOfTrack') && ...
            isequal(sicd_meta.SCPCOA.SideOfTrack, 'R')
        data_block = flipud(data_block); % View from above
    end

    writer_final_sicd.write_chip(data_block,[1 first_line_in_block]);
    first_line_in_block=line_range(2)+1;
end
clear writer_final_sicd; % Close file
if process_in_blocks
    reader_range_fft.close(); % Close file
    delete(filename_range_fft); % Intermediate file no longer needed
end

close(wb_hand); % Finally, close waitbar

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function cnn = init_cnn_model(varargin)
% cnn = init_cnn_model
% Initialize a CNN with caffe
%
% Optional arguments
% net_file   network binary file
% def_file   network prototxt file
% use_gpu    set to false to use CPU (default: true)
% use_caffe  set to false to avoid using caffe (default: true)
%            useful for running on the cluster (must use cached pyramids!)

% AUTORIGHTS
% ---------------------------------------------------------
% Copyright (c) 2014, Ross Girshick
%
% This file is part of the DeepPyramid code and is available
% under the terms of the Simplified BSD License provided in
% LICENSE. Please retain this notice and LICENSE if you use
% this file (or any portion of it) in your project.
% ---------------------------------------------------------

% ------------------------------------------------------------------------
% Options
ip = inputParser;

% network binary file
ip.addParamValue('net_file', ...
    './data/caffe_nets/ilsvrc_2012_train_iter_310k', ...
    @isstr);

% network prototxt file
ip.addParamValue('def_file', ...
    './model-defs/pyramid_cnn_output_conv5_scales_7_plane_1713.prototxt', ...
    @isstr);

% Set use_gpu to false to use the CPU
ip.addParamValue('use_gpu', true, @islogical);

% Set use_caffe to false to avoid using caffe
% (must be used in conjunction with cached features!)
ip.addParamValue('use_caffe', true, @islogical);

ip.parse(varargin{:});
opts = ip.Results;
% ------------------------------------------------------------------------

cnn.binary_file = opts.net_file;
cnn.definition_file = opts.def_file;
cnn.init_key = -1;
cnn.mu = get_channelwise_mean;
cnn.pyra.dimx = 1713;
cnn.pyra.dimy = 1713;
cnn.pyra.stride = 16;
cnn.pyra.num_levels = 7;
cnn.pyra.num_channels = 256;
cnn.pyra.scale_factor = sqrt(2);

if opts.use_caffe
  cnn.init_key = ...
      caffe('init', cnn.definition_file, cnn.binary_file);
  caffe('set_phase_test');
  if opts.use_gpu
    caffe('set_mode_gpu');
  else
    caffe('set_mode_cpu');
  end
end

% ------------------------------------------------------------------------
function mu = get_channelwise_mean()
% ------------------------------------------------------------------------
% load the ilsvrc image mean
data_mean_file = 'ilsvrc_2012_mean.mat';
assert(exist(data_mean_file, 'file') ~= 0);
% input size business isn't likley necessary, but we're doing it
% to be consistent with previous experiments
ld = load(data_mean_file);
mu = ld.image_mean; clear ld;
input_size = 227;
off = floor((size(mu,1) - input_size)/2)+1;
mu = mu(off:off+input_size-1, off:off+input_size-1, :);
mu = sum(sum(mu, 1), 2) / size(mu, 1) / size(mu, 2);


%COLLISIONS Conduct collision checking for SerialLink objects
%
% Uses the point data stored in the points property of SerialLink
% objects to conduct point-primitive collision checking with objects of
% the CollisionModel class. The function does not currently check the
% base of the SerialLink object!
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% This file requires file(s) from The Robotics Toolbox for MATLAB (RTB)
% by Peter Corke (www.petercorke.com), see file for statement
%
% Syntax:
%  (1) c = robot.collisions(q, cmdl)
%  (2) c = robot.collisions(q, cmdl, dyn_cmdl, dyn_T)
%  (3) c = robot.collisions(q, cmdl, dyn_cmdl)
%  (4) c = robot.collisions(q, [], ...)
%
%  (2) is as per (1) but also checks CollisionModel dyn_cmdl, whose
%       primitives move as specified by the frames in dyn_T
%  (3) is as per (2) but assumes dyn_cmdl is the robot's tool, i.e. the
%       the robot's end-effector frame (including tool transform) is
%       used in lieu of dyn_T
%  (4) is as per (2) or (3) but with no static objects
%
% Outputs:
%  c : mx1 vector, where m is the rows of q. Elements are false if no
%       collision exists or true if a collision exists
%
% Inputs:
%  q        : mxn matrix of m joint configurations of the robot, which
%              has n links
%  cmdl     : CollisionModel object, which is static
%  dyn_cmdl : CollisionModel object which move according to dyn_T
%  dyn_T    : Transforms applied to the primitives before the transform
%              of their own property being applied. The size of dyn_T
%              should be 4x4xmxp, where p is the number of primitives
%              in dyn_cmdl. The ith shape in dyn_cmdl corresponds to the
%              ith value of the 4th dimension. Use cat to create dyn_T.
%
% See also cat CollisionModel SerialLink

% 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/>.
%
% RTB LIBRARY:
%
% Copyright (C) 1993-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license

function c = collisions(robot, q, cmdl, dyn_cmdl, dyn_T)

% VERSION WITH BASE CHECKING
% pts = robot.points;
pts = robot.points(end-robot.n+1:end);
for i = length(pts): -1: 1
    numPts(i) = size(pts{i},1);
    pts{i}(:,4) = 1;
    pts{i} = pts{i}';
end

if isempty(cmdl), checkfuns = []; else checkfuns = cmdl.checkFuns; end
if nargin > 3
    dyn_checkfuns = dyn_cmdl.checkFuns;
    if nargin == 4 || isempty(dyn_T)
        tool = robot.tool;
        dyn_T = [];
    end
else
    dyn_checkfuns = [];
end

% VERSION WITH BASE CHECKING
% base = length(pts) - robot.n;
% switch base
%     case 0
%         % No base
%     case 1
%         T = robot.base;
%         trPts = T * pts{1};
%         points = trPts (1:3,:)';
%         if any(checkfuns{1}(points(:,1),points(:,2),points(:,3)))
%             C(:) = 1;
%             display('Base is colliding');
%             return;
%         end
%     otherwise
%         error('robot has missing or extra points');
% end

poses = size(q,1);
c = false(poses, 1);

nan = any(isnan(q),2);
c(nan) = true;
notnan = find(~nan)';

T0 = robot.base;

for p = notnan
    T = T0;
    prevPts = 0;
    for i = 1: robot.n;
        T = T * robot.links(i).A(q(p,i));
        if numPts(i) % Allows some links to be STLless
            %       VERSION WITH BASE CHECKING
            %       nextPts = prevPts+numPts(i+base);
            %       trPts(:,prevPts+1:nextPts) = T * pts{i+base};
            nextPts = prevPts+numPts(i);
            trPts(:,prevPts+1:nextPts) = T * pts{i};
            prevPts = nextPts;
        end
    end

    % Does same thing as cmdl.collision(trPoints(1:3,:)'), but
    % Does not have to access object every time - quicker
    for i = 1: length(checkfuns)
        if any(checkfuns{i}(trPts(1,:), trPts(2,:), trPts(3,:)))
            c(p) = true;
            break;
        end
    end

    % Then check the dynamic collision models, if any
    for i = 1: length(dyn_checkfuns)
        if isempty(dyn_T)
            dyn_trPts = T*tool \ trPts;
        else
            dyn_trPts = dyn_T(:,:,p,i) \ trPts;
        end
        if any(dyn_checkfuns{i}(dyn_trPts(1,:), dyn_trPts(2,:), ...
                dyn_trPts(3,:)))
            c(p) = true;
            break;
        end
    end

end
end