clear all;
close all;
clc;

% LIGAR CAMARA DEPTH
depthDevice = videoinput('kinect',2,'Depth_640x480');
preview(depthDevice);
pause(16)

% LIGAR CAMARA RGB
rgbDevice = videoinput('kinect',1,'RGB_640x480');

% TIRAR FRAME RGB
colorimg = getsnapshot(rgbDevice);
colorflipped = flip(colorimg,2); % IMAGEM RODADA

% TIRAR FRAME DEPTH
depthImage = getsnapshot(depthDevice);
depthflipped = flip(depthImage,2); % IMAGEM RODADA

% ALTURA DO CHÃO ATÉ AO CENTRO DA CAMARA
h = 0.7;

% EXTRAIR E MOSTRAR NUVEM DE PONTOS
xyzPoints = depthToPointCloud(depthImage,depthDevice,h);
figure(2);
showPointCloud(xyzPoints,'VerticalAxis','y','VerticalAxisDir','down');
hold off

% LABELING DOS EIXOS DA NUVEM DE PONTOS
xlabel('X(m)');
ylabel('Y(m)');
zlabel('Z(m)');

% MOSTRAR IMAGEM RGB
figure(3)
subplot(2,2,1)
imshow(colorflipped);
title('RGB Image')

% MOSTAR IMAGEM DEPTH EM ESCALA DE CINZENTOS
subplot(2,2,2)
imshow(im2gray(depthflipped),[])
title('Depth Image')

% RETIRAR OS PONTOS Z DA NUVEM DE PONTOS
z=double(squeeze(xyzPoints(:,:,3)));

%% TENTATIVA ELIMINAR O CHAO
% RETIRAR OS PONTOS CORRESPONDENTES ÀS ALTURAS
% PCy=double(squeeze(xyzPoints(:,:,2)));

% [My,Iy] = max(max(PCy));
% [Xy,Yy] = find(PCy==My);

% % RETIRAR OS PONTOS Y DA NUVEM DE PONTOS
% % OS PONTOS MAIS ELEVADOS EM Y SÃO OS MAIS JUNTOS AOS CHAO 
% YPmin=max(max(yaltura));        % PONTO MINIMO Y
% YPmax=min(min(yaltura));        % PONTO MAXIMO Y

% [xaltura,yaltura]=find(PCy<0.4);
% xyzPoints(PCy>0.39) = [];

%%
% MOSTRAR INDICE DOS PONTOS MINIMOS DA NUVEM DE PONTOS
[M,I] = min(min(z));

% LOCALIZAÇAO DO PONTO MAIS PROXIMO
[x,y]=find(z==M);


%% MOSTRAR IMAGEM 200x200 DO OBJETO MAIS PROXIMO (DENTRO DA RESOLUÇAO DA
% IMAGEM (480x640)
% [X(1:480),Y(1,640)]
x1min = min(x)-150;
x1max = min(x)+150;

% IMPEDIR QUE IMAGEM XMIN SEJA NEGATIVO E XMAX NAO ULTRAPASSE OS 480
if x1min<0
    x1min = 1;
elseif x1max>480
    x1max = 480;
end

y1min = min(y)-150;
y1max = min(y)+150;

% IMPEDIR QUE IMAGEM YMIN SEJA NEGATIVO EYXMAX NAO ULTRAPASSE OS 640
if y1min<0
    y1min = 1;
elseif y1max>640
    y1max=640;
end

x2 = x1min:x1max;
y2 = y1min:y1max;

hold off

% MOSTRAR IMAGEM OBJETO
a=colorflipped(x2,y2,:);
subplot(2,2,3);
imshow(a);
xlim([0 numel(x2)]);
ylim([0 numel(y2)]);
title('Selected Object')

hold on;

% MOSTRAR O PONTO ESCOLHIDO PARA DAR PLOT À IMAGEM
x3 = numel(x2)/2;
y3 = numel(y2)/2;

subplot(2,2,3);
plot(x3,y3,'r*','MarkerSize',5);
hold off

% MOSTRAR IMAGEM BINARIZADA
imgBin = imbinarize(im2gray(a));
subplot(2,2,4)
imshow(imgBin);
title('Binarized Image')

% [indices,dists] = findNearestNeighbors(ptCloud,double(squeeze([xyzPoints(225,323,1) xyzPoints(225,323,2) xyzPoints(225,323,3)])),10);