function V = Voxelize(X, voxelSize, plot)
% 3d voxelization of point cloud
% X: nx3 points
% voxelSize; voxel size
% plot: 'plot' or 'noplot'

% modified from Philipp Glira at TU Wien

% Di Wang, di.wang@aalto.fi
%%
OX = X;
% Preparations -----------------------------------------------------------------

% No. of points
noPoi = size(X,1);

% Logical indices for selection of points (true = selected)
%idxSelection = false(noPoi,1);

% Find voxel centers -----------------------------------------------------------

% Point with smallest coordinates
minPoi = min(X, [], 1);

% Rounded local origin for voxel structure (voxels of different pcs have coincident voxel centers if mod(100, voxelSize) == 0)
localOrigin = (floor(minPoi/100))*100;

% Find 3-dimensional indices of voxels in which points are lying
idxVoxel = [floor( (X(:,1)-localOrigin(1)) / voxelSize ) ...
    floor( (X(:,2)-localOrigin(2)) / voxelSize ) ...
    floor( (X(:,3)-localOrigin(3)) / voxelSize )];

% Remove multiple voxels
[idxVoxelUnique, ~, ic] = unique(idxVoxel, 'rows'); % ic contains "voxel index" for each point

% Coordinates of voxel centers
XVoxelCenter = [localOrigin(1) + voxelSize/2 + idxVoxelUnique(:,1)*voxelSize ...
    localOrigin(2) + voxelSize/2 + idxVoxelUnique(:,2)*voxelSize ...
    localOrigin(3) + voxelSize/2 + idxVoxelUnique(:,3)*voxelSize];

% No. of voxel (equal to no. of selected points)
noVoxel = size(XVoxelCenter,1);

UniSample = nan(noVoxel,1);
Corre = cell(noVoxel,1);

% Select points nearest to voxel centers ---------------------------------------

% Sort indices and points (in order to find points inside of voxels very fast in the next loop)
[ic, idxSort] = sort(ic);
X = X(idxSort,:);

idxJump = find(diff(ic));

% Example (3 voxel)
% ic         = [1 1 1 2 2 2 3]';
% diff(ic)   = [ 0 0 1 0 0 1 ]';
% idxJump    = [     3     6 ]';
%
% idxInVoxel = [1 2 3]; for voxel 1
% idxInVoxel = [4 5 6]; for voxel 2
% idxInVoxel = [7    ]; for voxel 3

for i = 1:noVoxel
    
    % Find indices of points inside of voxel (very, very fast this way)
    if i == 1
        idxInVoxel = [1:idxJump(i)];
    elseif i == noVoxel
        idxInVoxel = [idxJump(i-1)+1:noPoi];
    else
        idxInVoxel = [idxJump(i-1)+1:idxJump(i)];
    end
    
    % Distance of points to voxel center
    dist2voxelCenter = sqrt((X(idxInVoxel,1)-XVoxelCenter(i,1)).^2 + (X(idxInVoxel,2)-XVoxelCenter(i,2)).^2 + (X(idxInVoxel,3)-XVoxelCenter(i,3)).^2);
    
    % Find index of point with smallest distance to voxel center
    [~, idxSelectedPoi] = min(dist2voxelCenter);
    
    % Select point
    %idxSelection(idxSort(idxInVoxel(idxSelectedPoi))) = true;
    
    UniSample(i) = idxSort(idxInVoxel(idxSelectedPoi));
    Corre(i) = {idxSort(idxInVoxel)'};
    
    % 4Debug
    % idxInVoxel
    % idxSelectedPoi
    % XVoxelCenter(i,:)
    % X(idxInVoxel,:)
    % dist2voxelCenter
    % maxDist = sqrt(3*(voxelSize/2)^2);
    % correct(i) = all(dist2voxelCenter <= maxDist);
    
end
V.UniSample = UniSample;
V.Corre = Corre;




B = cell(size(XVoxelCenter,1),1);
for i = 1:size(XVoxelCenter,1)
    p1(1) =  XVoxelCenter(i,1) - voxelSize/2;
    p1(2) =  XVoxelCenter(i,2) - voxelSize/2;
    p1(3) =  XVoxelCenter(i,3) - voxelSize/2;
    
    p2(1) =  XVoxelCenter(i,1) + voxelSize/2;
    p2(2) =  XVoxelCenter(i,2) - voxelSize/2;
    p2(3) =  XVoxelCenter(i,3) - voxelSize/2;
    
    p3(1) =  XVoxelCenter(i,1) + voxelSize/2;
    p3(2) =  XVoxelCenter(i,2) + voxelSize/2;
    p3(3) =  XVoxelCenter(i,3) - voxelSize/2;
    
    p4(1) =  XVoxelCenter(i,1) - voxelSize/2;
    p4(2) =  XVoxelCenter(i,2) + voxelSize/2;
    p4(3) =  XVoxelCenter(i,3) - voxelSize/2;
    
    p5(1) =  XVoxelCenter(i,1) - voxelSize/2;
    p5(2) =  XVoxelCenter(i,2) - voxelSize/2;
    p5(3) =  XVoxelCenter(i,3) + voxelSize/2;
    
    p6(1) =  XVoxelCenter(i,1) + voxelSize/2;
    p6(2) =  XVoxelCenter(i,2) - voxelSize/2;
    p6(3) =  XVoxelCenter(i,3) + voxelSize/2;
    
    p7(1) =  XVoxelCenter(i,1) + voxelSize/2;
    p7(2) =  XVoxelCenter(i,2) + voxelSize/2;
    p7(3) =  XVoxelCenter(i,3) + voxelSize/2;
    
    p8(1) =  XVoxelCenter(i,1) - voxelSize/2;
    p8(2) =  XVoxelCenter(i,2) + voxelSize/2;
    p8(3) =  XVoxelCenter(i,3) + voxelSize/2;
    
    %p = [p1;p2;p3;p4;p1;p5;p6;p7;p8;p5;p1;[nan nan nan];p2;p6;[nan nan nan];p3;p7;[nan nan nan];p4;p8];
    
    B{i} = [p1;p2;p3;p4;p5;p6;p7;p8];
    %plot3(p(:,1),p(:,2),p(:,3));
end

V.Box = B; 

switch plot
    case 'plot'
        figure('units','normalized','outerposition',[0 0 1 1])
        hold all;
        for i = 1:size(XVoxelCenter,1)
            t = B{i};
            p1 = t(1,:);
            p2 = t(2,:);
            p3 = t(3,:);
            p4 = t(4,:);
            p5 = t(5,:);
            p6 = t(6,:);
            p7 = t(7,:);
            p8 = t(8,:);
            
            p = [p1;p2;p3;p4;p1;p5;p6;p7;p8;p5;p1;[nan nan nan];p2;p6;[nan nan nan];p3;p7;[nan nan nan];p4;p8];
            
            plot3(p(:,1),p(:,2),p(:,3),'Linewidth',.5);
        end
        
        T = Corre;
        cols = lines(length(T));
        pt_v = cell(length(T),1);
        col = cell(length(T),1);
        for i = 1:length(T)
            pt = OX(T{i},:);
            pt_v(i) = {pt};
            col(i) = {repmat(cols(i,:),size(pt,1),1)};
            
        end
        hold on;
        pcshow(cell2mat(pt_v),cell2mat(col),'markersize',.2);
        hold off
        xlabel('X(m)');ylabel('Y(m)');zlabel('Z(m)');
        axis image, view(3)
        title('Spatial tiles')
        pause(.1)
    otherwise
        %disp('voxelization done !')
        return
end

end