%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% file: example2DETH.m                                                    %
%                                                                         %
% brief: This is an example for getting a distance, amplitude and 2D      %
% color 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 = 5;
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 2D camera resolution
[status] = BTAwriteRegister(deviceHandle, 224, 32); 
if status ~= 0
    statusClose = BTAclose(deviceHandle);
    error('BTAwriteRegister 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

%wait some time that the changes take effect
pause(3); 

colorDataAvailable = 0;

%It is possible that a received frame doesn't contain 2D color data.
%The reason for this are different frame rates of the tof and the 2d sensor.
%For this reason the next lines of code get repeated until a frame arrvies which contains 2D color data.            
while (~colorDataAvailable)
    % 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 distance data
    [status, distData, integrationTime, modulationFrequency, unit] = BTAgetDistances(frameHandle);
    if status ~= 0
        statusFree = BTAfreeFrame(frameHandle);
        statusClose = BTAclose(deviceHandle);
        error('BTAgetDistances failed. See "status" for error description');
    end

    % get amplitude data
    [status, ampData, integrationTime2, modulationFrequency2, unit2] = BTAgetAmplitudes(frameHandle);
    if status ~= 0
        statusFree = BTAfreeFrame(frameHandle);
        statusClose = BTAclose(deviceHandle);
        error('BTAgetAmplitudes failed. See "status" for error description');
    end

    % get 2D color data
    [status, colorData2D, integrationTime, modulationFrequency, unit] = BTAget2DData(frameHandle);
    if status ~= 0
        if status == 1
            % there are no 2D data in this frame. this gets caused by different
            % frame rates of the tof and the 2d sensor.
            colorDataAvailable = 0;
        else
            statusFree = BTAfreeFrame(frameHandle);
            statusClose = BTAclose(deviceHandle);
            error('BTAget2DData failed. See "status" for error description');
        end
    else
       colorDataAvailable = 1; 
    end

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

%create window for distances
figure('name','Distance data');
set(gcf,'NumberTitle','off')
%set color range from 0 to 3000
clims  = [0 3000];
hDist = imagesc(distData, clims);
colorbar;
cm=flipud(jet);
colormap(cm);
global MK;
MK=0;
set(gcf,'KeyPressFcn','MK=1;');

%create window for amplitudes 
figure('name','Amplitude data');
set(gcf,'NumberTitle','off')
%set color range from 0 to 5000 
clims  = [0 5000];
hAmp=imagesc(ampData, clims);
colorbar;
colormap('GRAY');
global MK2;
MK2=0;
set(gcf,'KeyPressFcn','MK2=1;');

%create window for 2D color data 
figure('name','2D color data');
set(gcf,'NumberTitle','off')
%parse 2D data
imgR = uint8((255/31)*bitshift(bitand(colorData2D,63488),-11));
imgG = uint8((255/63)*bitshift(bitand(colorData2D,2016),-5));
imgB = uint8((255/31)*bitand(colorData2D,31));
imgRGB = cat(3,imgR,imgG,imgB);
h2DData=imshow(imgRGB);
global MK3;
MK3=0;
set(gcf,'KeyPressFcn','MK3=1;');

ampWindowIsclosed = 0;
distWindowIsclosed = 0;
colorDataWindowIsclosed = 0;
%loop 
while ~MK && ~MK2 && ~MK3 &&~(ampWindowIsclosed && distWindowIsclosed && colorDataWindowIsclosed)
    % get one frame
    [status, frameHandle,  frameCounter, timeStamp] = BTAgetFrame(deviceHandle, 500);
    if status == 0
        %get distances
        [status, distData, integrationTime, modulationFrequency, unit] = BTAgetDistances(frameHandle);
        if status == 0
            if ishandle(hDist)
                %display distances
                set(hDist,'CData',distData);
                refreshdata(hDist);
            else
                distWindowIsclosed = 1;
            end
        end
        %get amplitudes
        [status, ampData, integrationTime2, modulationFrequency2, unit2] = BTAgetAmplitudes(frameHandle);
        if status == 0
            if ishandle(hAmp)
                %display amplitudes
                set(hAmp,'CData',ampData);
                refreshdata(hAmp);
            else
                ampWindowIsclosed = 1;   
            end
        end
        %get 2D color data
        [status, colorData2D, integrationTime, modulationFrequency, unit] = BTAget2DData(frameHandle);
        if status == 0
            if ishandle(h2DData)
                %parse 2D color data
                imgR = uint8((255/31)*bitshift(bitand(colorData2D,63488),-11));
                imgG = uint8((255/63)*bitshift(bitand(colorData2D,2016),-5));
                imgB = uint8((255/31)*bitand(colorData2D,31));
                imgRGB = cat(3,imgR,imgG,imgB);
                %display 2D color data
                set(h2DData,'CData',imgRGB);
                refreshdata(h2DData);
            else
                colorDataWindowIsclosed = 1;   
            end
        end
        %free frame
        [status] = BTAfreeFrame(frameHandle);
        if status ~= 0
            error('BTAfreeFrame failed. See "status" for error description');
        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;



