%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% file: pointcloudUSB.m                                                   %
%                                                                         %
% brief: This is an example for for getting a pointcloud stream.          %
%                                                                         %
% autor: Benedikt Radl                                                    %
%                                                                         %
% Copyright (c) 2014 Bluetechnix GmbH                                     %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;
clear all;

% get a config structure
[status, configStruct] = BTAinitConfig;
if status ~= 0
    error('BTAinitConfig failed. See "status" for error description');
end

% fill the config struct
configStruct.frameMode = 3;
configStruct.serialNumber = 0;

% open device
[status, deviceHandle] = BTAopen(configStruct);
if status ~= 0
    error('BTAopen failed. See "status" for error description');
end

% set framerate to 5 (for limiting the workload)
status = BTAsetFrameRate(deviceHandle, 5);
if status ~= 0
    statusClose = BTAclose(deviceHandle);
    error('BTAsetFrameRate failed. See "status" for error description');
end
% set framemode to XYZ-coordinates
status = BTAsetFrameMode(deviceHandle, 3);
if status ~= 0
    statusClose = BTAclose(deviceHandle);
    error('BTAwriteRegister failed. See "status" for error description');
end
%wait some time that the changes take effect
pause(2); 

% get one frame
[status, frameHandle, frameCounter, timeStamp] = BTAgetFrame(deviceHandle, 500);
if status ~= 0
    statusClose = BTAclose(deviceHandle);
    error('BTAgetFrame failed. See "status" for error description');
end

% get xyz coordinates
[status, xData, yData, zData, integrationTime, modulationFrequency, unit] = BTAgetXYZcoordinates(frameHandle);
if status ~= 0
    statusFree = BTAfreeFrame(frameHandle);
    statusClose = BTAclose(deviceHandle);
    error('BTAgetXYZcoordinates failed. See "status" for error description');
end

% free frame
[status] = BTAfreeFrame(frameHandle);
if status ~= 0
    statusClose = BTAclose(deviceHandle);
    error('BTAfreeFrame failed. See "status" for error description');
end

%mask invalid coordinates and mirror x and y data
valid = zData > 0.1 & zData <= 2;
validXData = xData(valid) * (-1);
validYData = yData(valid) * (-1);
validZData = zData(valid);

%create window for pointcloud
figure('name','Pointcloud');
set(gcf,'NumberTitle','off')
%draw image
h = scatter3(validZData,validXData,validYData,10,validZData,'filled');

%limit axis
xlim('manual');
xlim([0 2]);
ylim('manual');
ylim([-1 1]);
zlim('manual');
zlim([-1 1]);

%set colormap
colormap(flipud(jet));

%limit color range
caxis([0 2]);

%axis descriptions
xlabel('depth');
ylabel('width');
zlabel('height');

global MK;
MK=0;
set(gcf,'KeyPressFcn','MK=1;');


windowIsclosed = 0;
%loop 
while ~MK && ~windowIsclosed
    % get one frame
    [status, frameHandle, frameCounter, timeStamp] = BTAgetFrame(deviceHandle, 0);
    if status == 0
        % get xyz coordinates
        [status, xData, yData, zData, integrationTime, modulationFrequency, unit] = BTAgetXYZcoordinates(frameHandle);
        if status == 0
            %mask invalid coordinates and mirror x and y axis
            valid = zData > 0.1 & zData <= 2;
            validXData = xData(valid) * (-1);
            validYData = yData(valid) * (-1);
            validZData = zData(valid);

            
            if ishandle(h)
                if numel(validZData) > 5
                    %display
                    set(h,'XDataSource','validZData');
                    set(h,'YDataSource','validXData');
                    set(h,'ZDataSource','validYData');
                    set(h,'CDataSource','validZData');
                    
                    refreshdata(h); 
                else
                    delete(findobj(gca, 'type', 'patch'));
                end
            else
                windowIsclosed = 1;
            end
            
            % free frame
            [status] = BTAfreeFrame(frameHandle);
            if status ~= 0
                error('BTAfreeFrame failed. See "status" for error description');
            end
        end 
    end  
    pause(0.1);   
end
 
%close device
status = BTAclose(deviceHandle);
if status ~= 0
	error('BTAclose failed. See "status" for error description');
end

close all;
clear all;
