%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% file: examplePointcloudETH.m                                            %
%                                                                         %
% brief: This is an example for for getting a pointcloud stream.          %
%                                                                         %
% autor: Benedikt Radl                                                    %
%                                                                         %
% Copyright (c) 2015 Bluetechnix Group 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.udpDataIpAddr = [224, 0, 0, 1];
configStruct.udpDataIpAddrLen = 4;
configStruct.udpDataPort = 10002;
configStruct.tcpDeviceIpAddr = [192, 168, 0, 10];
configStruct.tcpDeviceIpAddrLen = 4;
configStruct.tcpControlPort = 10001;
configStruct.frameMode = 1;
configStruct.frameQueueMode = 1;
configStruct.frameQueueLength = 5;

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

% set the interval of the keep alive massages 
status = BTAsetKeepAliveMsgInterval(deviceHandle, 2);
if status ~= 0
    statusClose = BTAclose(deviceHandle);
    error('BTAsetKeepAliveMsgInterval 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 > 1 & zData <= 3000;
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 3000]);
ylim('manual');
ylim([-1500 1500]);
zlim('manual');
zlim([-1500 1500]);

%set colormap
colormap(flipud(jet));

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

%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, 500);
    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 > 1 & zData <= 3000;
            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;
