close all
clear all
clc

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

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

% RETIRAR FRAMES DE AMBAS AS CAMARAS 
RGBFrame = getsnapshot(RGBDisp);
DEPTHFrame = getsnapshot(DEPTHDisp);

% AMBOS FRAMES TIVERAM DE SER RODADOS PARA FICAREM NORMAIS
% O FRAME PARA A NUVEM DE PONTOS NÃO PODE SER RODADO
RGBFrameFlipped = flip(RGBFrame,2);
DEPTHFrameFlipped = flip(DEPTHFrame,2);

%% NUVEM DE PONTOS COMPLETA
% EXTRAIR A NUVEM DE PONTOS
ptCloudOriginal = pcfromkinect(DEPTHDisp, DEPTHFrame, RGBFrame);

% RODAR A NUVEM DE PONTOS PARA FICAR PARALELA COM EIXO DOS XX
% ANGULO DE ROTAÇAO
theta = -0.04;

% MATRIZ DE ROTAÇAO EM X
Rx = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)];
trans = [0, 0, 0];
tform = rigid3d(Rx,trans);

% NOVA NUVEM DE PONTOS RODADA
ptCloudRot = pctransform(ptCloudOriginal,tform);

% MOSTART NUVEM DE PONTOS
figure
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down");
title('Rotated Point Cloud')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')


%% ENCONTRAR VIZINHANÇA DO OBJETO MAIS PROXIMO
% RETIRAR A MATRIZ DE PONTOS DE LOCALIZAÇAO DOS PONTOS
zRot = ptCloudRot.Location(:,:,3);

% MOSTART INDICE DOS PONTOS MINIMOS DA NUVEM DE PONTOS
[MRot,~] = min(min(zRot));

% PONTO ORIGEM PARA PROCEDER À VIZINHANÇA -> (0,0,0) ERA O PONTO QUE MARCAVA "CENTRO" DO OBJETO
point = [0 0 0];

% INTRODUZIR A DISTANCIA DO PONTO MAIS PROXIMO ATÉ AO PONTO MAIS AFASTADO DO OBJETO
Z = 0.25;

% Nº DE PONTOS DA VIZINHANÇA
K = numel(find(zRot<(MRot+Z)));

% LOCALIZAÇÃO DOS PONTOS DE VIZINHANÇA
[indices,~] = findNearestNeighbors(ptCloudRot,point,K);

% NUVEM DE PONTOS DO OBJETO MAIS PROXIMO
ptCloudObjeto = select(ptCloudRot,indices);

% MOSTRAR A NUVEM DE PONTOS COM O OBJETO SELECIONADO
figure
pcshow(ptCloudRot,'VerticalAxis','Y',VerticalAxisDir='Down')
hold on
pcshow(ptCloudObjeto.Location,'ob','MarkerSize',5,'VerticalAxis','Y',VerticalAxisDir='Down')

title('Selected Object Points')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

%% OBTER IMAGEM DO OBJETO MAIS PRÓXIMO
% TRANSFORMAR OS INDICES EM COLUNAS E LINHAS
[row,col] = ind2sub([480 640],indices);

% ENCONTRAR OS LIMITES DO OBJETO
rowmax = max(max(row))+10;
rowmin = min(min(row))-10;
colmax = max(max(col))+10;
colmin = min(min(col))-10;

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

% IMPEDIR QUE IMAGEM YMIN SEJA NEGATIVO E YMAX NAO ULTRAPASSE OS 640
if colmin < 0
    colmin = 1;
elseif colmax > 640
    colmax = 640;
end

% FRAME RGB RECORTADO PARA ELIMINAR PERTURBAÇÕES NO FUNDO
RGBCropped = flip(RGBFrameFlipped(rowmin:rowmax,colmin:colmax,:),2);

% MOSTRAR IMAGEM DO OBJETO RECORTADO
figure
imshow(RGBCropped)


%% CONTORNOS
% LINHAS E COLUNAS DA MATRIZ DO OBJETO SELECIONADO
[row,col] = ind2sub([480 640],indices);

% CONVERTER IMAGEM RGB EM PRETOS
BlackRGB=zeros(size(RGBFrameFlipped));

% OS PONTOS DO OBJETO COINCIDENTES COM A IMAGEM RGB SÃO CONVERTIDOS A BRANCO
for i=1:size(col)
    BlackRGB(row(i),col(i),:)=1;
end

% figure
% imshow(BlackRGB)

bi = imbinarize(im2gray(BlackRGB));
figure
imshow(bi);

p=bwperim(bi);
figure
imshow(p)




% ptCloudRotCol = ptCloudRot.Location(1,:);
% ptCloudRotLin = ptCloudRot.Location(:,1);
% ptCloudRotLoc = ptCloudRot.Location;
% ptCloudObjetoLoc = ptCloudObjeto.Location;

% for ptCloudRotCol = ptCloudRotLoc(1,:)
%     for ptCloudRotLin = ptCloudRotLoc(:,1)
%         ptCloudRot = 0;
%         if ptCloudRotLoc == ptCloudObjetoLoc
%             ptCloudRot = 1;
%         end
%     end
% end


% fff=RGBFrameFlipped;

% for i=1:1:size(RGBFrameFlipped,2)
%     for j=1:1:size(RGBFrameFlipped,1)
% 
%         fff(row(i),col(j),:)=0;
%         %  if(i==row(i) && j==col(i))
%         %      fff(j,i,:)=1;
%         %  else
%         %     fff(j,i,:)=0;
%         % end
%     end
% end

% figure
% imshow(fff)

% for i=1:size(RGBFrameFlipped,1)
%     for j=1:size(RGBFrameFlipped,2)
%         fff(i,j,:) = 0;
%         fff(indices)=1;
%         if fff(i,j) == MatrixObjeto(i,j)
%             fff(i,j,:) = 1;
%         end
%     end
% end

% figure
% imshow(fff)

% fff=RGBFrameFlipped;

% for i=1:1:size(row,1)
%     fff(row(i),col(i),i)=1;
% end

% figure
% imshow(fff)