clc
clf
clear all
close all
clear class

%% Parameters
Device = 'kinect';  % vyber snimace
Lighting = 0.1; 	% {0-1} nasviceni pro kalibraci krabice

BoxMin = 30;        % min limit vysky okraje piskoviste
BoxMax = 130;       % max limit vysky okraje piskoviste

% orez snimaneho obrazu (v pixelech hloubkoveho obrazu)
CropUp = 30;        % orez snimaneho obrazu zvrchu
CropDown = 130;     % orez snimaneho obrazu zespodu
CropLeft = 0;       % orez snimaneho obrazu zleva
CropRight = 180;	% orez snimaneho obrazu zprava

% posun stran vysledneho obrazu
MoveUp = -10;       % posun horni strany vys  ledneho promitani dolu -10
MoveBottom = 15;	% posun dolni strany vysledneho promitani nahoru 15
MoveLeft = 30;      % posun leve strany vysledneho promitani doprava 0
MoveRight = -20;    % posun prave strany vysledneho promitani doleva -10

% funkce pro filtraci barvy piskoviste
% pro vytvoreni vlastni funkce lze pouzit Color Thresholder na ColorImageBOX
BoxMaskFunction = "createMaskGreenLAB";     

%% Projection
MonitorPositions = get(0, 'MonitorPositions'); 

fig1 = figure('Units','pixels',...
                            'ToolBar','none',...
                            'MenuBar','none',...
                            'Position',[MonitorPositions(2,1) MonitorPositions(2,2) MonitorPositions(2,3) MonitorPositions(2,4)],...
                            'Visible','on',...
                            'Color','black'); 
axes(fig1,'Units','pixels',...
                            'Position',[1 1 MonitorPositions(2,3) MonitorPositions(2,4)],...
                            'Visible','off',...
                            'ZLim',[0 300],...
                            'CLim',[0 300]);

%% Snapshot acquisition
DepthVid = videoinput(Device, 2);
DepthShot = getsnapshot(DepthVid);
DeviceDepX = size(DepthShot,2);
DeviceDepY = size(DepthShot,1);

% podsviceni pro porizeni snimku piskoviste
BGimage=zeros(MonitorPositions(2,4),MonitorPositions(2,3),3);
BGimage(:,:,:)=Lighting;
imshow(BGimage);
pause(1);

ColorVid = videoinput(Device, 1);
ColorShotBOX = getsnapshot(ColorVid);
DeviceColX = size(ColorShotBOX,2);
DeviceColY = size(ColorShotBOX,1);

% podsviceni pro porizeni snimku ramecku projekce
BGimage(:,:,:) = 0;
BGimage(:,1:20,':') = 1;
BGimage(:,(MonitorPositions(2,3)-20):(MonitorPositions(2,3)-1),':') = 1;
BGimage(1:20,:,':') = 1;
BGimage((MonitorPositions(2,4)-20):(MonitorPositions(2,4)-1),:,':') = 1;
imshow(BGimage);
pause(1);

ColorShotFRAME = getsnapshot(ColorVid);

%% Crop
CropUp = CropUp+1;
CropDown = DeviceDepY-CropDown;
CropLeft = DeviceDepX-CropLeft;
CropRight = CropRight+1;

DepthImage = flip(DepthShot(CropUp:CropDown,CropRight:CropLeft),2);
DepthImage(DepthImage==0) = mean(DepthImage(:));
DepthImage(DepthImage<(mean(DepthImage(:))-BoxMax)) = mean(DepthImage(:))-BoxMax;

ColorImageFRAME = flip(ColorShotFRAME(...
                round(CropUp/DeviceDepY*DeviceColY):round(CropDown/DeviceDepY*DeviceColY)... 
                ,round(CropRight/DeviceDepX*DeviceColX):round(CropLeft/DeviceDepX*DeviceColX)... 
                ,':'),... 
                2);    
            
ColorImageBOX = flip(ColorShotBOX(...
                round(CropUp/DeviceDepY*DeviceColY):round(CropDown/DeviceDepY*DeviceColY)... 
                ,round(CropRight/DeviceDepX*DeviceColX):round(CropLeft/DeviceDepX*DeviceColX)... 
                ,':'),... 
                2);            

%% Convert to bin
% prevod barvy na bin podle barevne filtrace
[ColorImageFRAME_BIN,~] = createMaskWhiteLAB(ColorImageFRAME);
createMaskBOX = str2func(BoxMaskFunction);
[ColorImageBOX_BIN,~] = createMaskBOX(ColorImageFRAME);

[ColY,ColX,~] = size(ColorImageFRAME_BIN);

% prevod hloubky na bin podle bodu nachazejicich se v urcite vysce nad podlahou
DepthImage_BIN = DepthImage;
DepthImage_BIN(DepthImage_BIN>=(mean(DepthImage(:))-BoxMin)|DepthImage_BIN<(mean(DepthImage(:))-BoxMax)) = [0];
DepthImage_BIN(DepthImage_BIN<(mean(DepthImage(:))-BoxMin)&DepthImage_BIN>=(mean(DepthImage(:))-BoxMax)) = [1];

fig2 = figure('Position',[MonitorPositions(1,1) MonitorPositions(1,2)+MonitorPositions(1,4)/2 MonitorPositions(1,3) MonitorPositions(1,4)/2-80]); 

%% DEPTH boundaries
[intersect_Depth_ULx,intersect_Depth_ULy,intersect_Depth_URx,intersect_Depth_URy,intersect_Depth_DLx,intersect_Depth_DLy,intersect_Depth_DRx,intersect_Depth_DRy,DepthImage_filled] = rectangleCorners(DepthImage_BIN,1,3,1);

%% FRAME boundaries
[intersect_Frame_ULx,intersect_Frame_ULy,intersect_Frame_URx,intersect_Frame_URy,intersect_Frame_DLx,intersect_Frame_DLy,intersect_Frame_DRx,intersect_Frame_DRy,~] = rectangleCorners(ColorImageFRAME_BIN,1,3,2);

%% Transform ColorImage
TFORM=cp2tform([intersect_Frame_ULx intersect_Frame_ULy;...
    intersect_Frame_DLx intersect_Frame_DLy;...
    intersect_Frame_URx intersect_Frame_URy;...
    intersect_Frame_DRx intersect_Frame_DRy],...
    [1 1; 1 ColY; ColX 1; ColX ColY],'projective'); 

CutoutBOX=imtransform(ColorImageBOX_BIN,TFORM,'XData',...
    [1 ColX],'YData',[1 ColY],'XYScale',[1 1]);

%% BOX boundaries
[intersect_Box_ULx,intersect_Box_ULy,intersect_Box_URx,intersect_Box_URy,intersect_Box_DLx,intersect_Box_DLy,intersect_Box_DRx,intersect_Box_DRy,~] = rectangleCorners(CutoutBOX,1,3,3);

%% Transform DepthImage
DepthImage_MSK = DepthImage;
DepthImage_MSK(DepthImage_filled==0) = min(DepthImage(:));
TFORM=cp2tform([intersect_Depth_ULx intersect_Depth_ULy;...
    intersect_Depth_DLx intersect_Depth_DLy;...
    intersect_Depth_URx intersect_Depth_URy;...
    intersect_Depth_DRx intersect_Depth_DRy],...
    [intersect_Box_ULx intersect_Box_ULy;...
    intersect_Box_DLx intersect_Box_DLy;...
    intersect_Box_URx intersect_Box_URy;...
    intersect_Box_DRx intersect_Box_DRy],'projective'); 
CutoutDepth=imtransform(DepthImage_MSK,TFORM,'XData',[1 ColX],'YData',[1 ColY],'XYScale',[1 1]);
CutoutDepth_BIN=imtransform(DepthImage_filled,TFORM,'XData',[1 ColX],'YData',[1 ColY],'XYScale',[1 1]);
CutoutDepth(CutoutDepth_BIN~=1) = [0];

%% Final cropping/padding
CropUp = 0;
CropBottom = 0;
CropLeft = 0;
CropRight = 0;

if MoveUp<0
    CropUp = -MoveUp;
    MoveUp = 0;
end

if MoveBottom<0
    CropBottom = -MoveBottom;
    MoveBottom = 0;
end

if MoveLeft<0
    CropLeft = -MoveLeft;
    MoveLeft = 0;
end

if MoveRight<0
    CropRight = -MoveRight;
    MoveRight = 0;
end

DepthProjected = padarray(padarray(CutoutDepth,[MoveUp MoveLeft],min(DepthImage(:)),'pre'),[MoveBottom MoveRight],min(DepthImage(:)),'post');

[DepthPaddedY,DepthPaddedX,~] = size(DepthProjected);

DepthProjected(:,[1:CropLeft,(DepthPaddedX-CropRight):DepthPaddedX],':')=[];
DepthProjected([1:CropUp,(DepthPaddedY-CropBottom):DepthPaddedY],:,':')=[];

DepthProjected(DepthProjected>=max(DepthImage_MSK(:))|DepthProjected<min(DepthImage_MSK(:))) = min(DepthImage(:));
DepthProjected(DepthProjected>=max(DepthImage_MSK(:))|DepthProjected<min(DepthImage_MSK(:))) = min(DepthImage(:));

%% Results
figure(1);               % promitaci plocha
imagesc(DepthProjected); % zobrazeni hloubkove mapy
cmap = parula(256);
cmap(1,:)=[0,0,0];       % cerna barva nejnizsich hodnot
colormap(cmap);

delete(ColorVid);
delete(DepthVid);