cosmo vol coordinates

function [transformed, outside] = cosmo_vol_coordinates(ds, coords)
    % convert to and from spatial (x,y,z) coordinates
    %
    % [transformed, outside]=cosmo_vol_coordinates(ds[, coords])
    %
    % Inputs:
    %   ds              dataset struct with fields .a.fdim.{labels,values}
    %                   (with {'i','j','k'} a subset of .a.fdim.labels) and
    %                   .a.vol.{mat,dim}
    %   coords          (Optional) one of:
    %                   - 1xP matrix of feature indices
    %                   - 3xP matrix with spatial (x,y,z) coordinates
    %                   If omitted then it is set to 1:nfeatures, with
    %                   nfeatures=size(ds.samples,2);
    %
    % Returns:
    %   transformed     - If coords is 1xP: 3xP spatial coordinates of the
    %                     feature indices; columns have NaN for feature ids
    %                     not in the dataset.
    %                   - If coords is 3xP: 1xP feature indices of the
    %                     coordinates; an entry is 0 for coordinates not
    %                     matching a feature.
    %
    % #   For CoSMoMVPA's copyright information and license terms,   #
    % #   see the COPYING file distributed with CoSMoMVPA.           #

    % if no coords, return coordinates of all features
    if nargin < 2
        coords = zeros(1, 0);
    end

    check_input(ds);

    % convert either from or to coordinates
    one_or_three = size(coords, 1);
    switch one_or_three
        case 1
            [transformed, outside] = lin2xyz(ds, coords);
        case 3
            [transformed, outside] = xyz2fa_indices(ds, coords);
        otherwise
            error('Input must be 1xP or 3xP ');
    end

function [xyz, outside] = lin2xyz(ds, coords)
    nfeatures = size(ds.samples, 2);
    if isempty(coords)
        coords = 1:nfeatures;
    end

    % exclude invalid indices
    outside = coords < 1 | coords > nfeatures | round(coords) ~= coords;
    inside = ~outside;
    nfeatures = numel(outside);

    % deal with valid indices
    ds_inside = cosmo_slice(ds, coords(inside), 2);

    % get dataset fa indices
    [fa_ijk, unused, ndim] = get_fa_ijk_indices(ds_inside);

    % store indices in matrix with 4 rows
    ijk = zeros(ndim + 1, nfeatures);
    for k = 1:ndim
        ijk(k, :) = fa_ijk{k};
    end

    % set last row to 1
    ijk(ndim + 1, :) = 1;

    % convert to xyz
    xyz1 = ds.a.vol.mat * ijk;

    % set elements inside & outside
    ncoords = numel(coords);
    xyz = NaN(ndim, ncoords);
    xyz(:, inside) = xyz1(1:ndim, :);

function [lin2fa, outside] = xyz2fa_indices(ds, coords)
    % ijk indices in dataset
    [fa_ijk, dim_sizes, ndim] = get_fa_ijk_indices(ds);

    % add row of ones to coordinates
    nfeatures = size(coords, 2);
    coords1 = [coords; ones(1, nfeatures)];

    % convert ijk indices to coordinates
    % TODO: check use of 'round'
    ijk1 = round(ds.a.vol.mat \ coords1);

    % store ijk indices in cell
    coords_ijk_cell = cell(1, ndim);
    outside = false(1, nfeatures);
    for k = 1:ndim
        outside = outside | ijk1(k, :) < 1 | ijk1(k, :) > dim_sizes(k);
    end

    for k = 1:ndim
        coords_ijk_cell{k} = ijk1(k, ~outside);
    end

    % convert to linear indices
    coords_lin = sub2ind(dim_sizes, coords_ijk_cell{:});
    fa_lin = sub2ind(dim_sizes, fa_ijk{:});

    if ~isequal(unique(fa_lin), sort(fa_lin))
        unq = unique(fa_lin);
        h = histc(fa_lin, unq);
        idx = find(h > 1, 1);
        pos = find(fa_lin == unq(idx), 2);
        error('Duplicate feature index at %d and %d', pos(1), pos(2));
    end

    % compute mapping from coords indices to fa indices
    nfull = prod(dim_sizes);
    all2fa_lin = zeros(1, nfull);
    all2fa_lin(fa_lin) = 1:numel(fa_lin);

    % apply mapping
    lin2fa = zeros(1, nfeatures);
    lin2fa(~outside) = all2fa_lin(coords_lin);
    outside = outside | lin2fa == 0;

function [fa_ijk, dim_sizes, ndim] = get_fa_ijk_indices(ds)
    % helper: return a cell with i, j, k indices

    dim_labels = {'i', 'j', 'k'};
    ndim = numel(dim_labels);

    fa_ijk = cell(1, ndim);
    dim_sizes = zeros(1, ndim);
    for k = 1:ndim
        dim_label = dim_labels{k};
        [dim, dim_idx, attr_name, dim_name] = cosmo_dim_find(ds, ...
                                                             dim_label, true);
        if dim ~= 2
            error('Unsupported label %s in sample dimension', dim_label);
        end
        dim_values = ds.a.(dim_name).values{dim_idx};
        fa_ijk{k} = dim_values(ds.(attr_name).(dim_label));
        dim_sizes(k) = numel(dim_values);
    end

    if ~isequal(dim_sizes, ds.a.vol.dim)
        error('size mismatch between .a.vol.dim and .a.fdim.values');
    end

function check_input(ds)
    % check input dataset
    cosmo_check_dataset(ds);
    if isstruct(ds) && isfield(ds, 'a') && ...
            isfield(ds.a, 'vol') && isfield(ds.a.vol, 'mat')
        mat = ds.a.vol.mat;
    else
        error('missing fields .a.vol.mat');
    end

    % check matrix
    if ~isnumeric(mat) || ~isequal(size(mat), [4 4])
        error('matrix must be 4x4');
    end