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.055;

% 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);

figure
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")

%% TARGET AREA IDENTIFICATION
stepSize = 100;

indices1 = 1:stepSize:ptCloudRot.Count;
tempPtCloud = select(ptCloudRot,indices1);

[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);

worldPoints = tempPtCloud.Location;

[Y,X] = ind2sub([size(ptCloudRot.Location,1),size(ptCloudRot.Location,2)],indices1);
imagePoints = [X(validIndices)' Y(validIndices)'];

camMatrix = estimateCameraMatrix(imagePoints,worldPoints);

roi1 = [-0.1 0.1 -0.3 0.35 1.1 1.3];

indices1 = findPointsInROI(ptCloudRot,roi1,camMatrix);
ptcloudB = select(ptCloudRot,indices1);

figure
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
pcshow(ptcloudB.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")
title('Point Cloud w/Target Area')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

p1 = [-0.5 0.415 1.7]';
p2 = [-0.5 0.415 1.2]';
p3 = [0.5 0.415 1.2]';
p4 = [0.5 0.415 1.7]';
p5 = [-0.5 0 1.7]';
p6 = [-0.5 0 1.2]';
p7 = [0.5 0 1.2]';
p8 = [0.5 0 1.7]';
A1=[p1 p2 p3 p4;p4 p3 p7 p8;p7 p8 p5 p6;p7 p6 p2 p3;p6 p5 p1 p2;p8 p5 p1 p4];

p=fill3(A1(1,:),A1(2,:),A1(3,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(4,:),A1(5,:),A1(6,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(7,:),A1(8,:),A1(9,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(10,:),A1(11,:),A1(12,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(13,:),A1(14,:),A1(15,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(16,:),A1(17,:),A1(18,:),'--w');
p.FaceAlpha = 0.3;

hold off

%% REMOVER O PLANO DO CHAO
% SET THE MAX POINT TO PLABE DISTANCE FOR PLANE FITTING
maxDistance = 0.0005;     % (m)

% SET THE NORMAL VECTOR OF THE PLANE
referenceVector = [0,1,0];

% DETECT THE FIRST PLANE AND EXTRACT IT FROM THE POINT CLOUD
[~,~,outlierIndices] = pcfitplane(ptcloudB,maxDistance,referenceVector);
remainPtCloud = select(ptcloudB,outlierIndices);

% REMOVER O BARULHO DA NUVEM DE PONTOS (ELIMINAR "IMPUREZAS")
pclimpa = pcdenoise(remainPtCloud,"NumNeighbors",50);

% MOSTRAR A NUVEM DE PONTOS LIMPA
figure
pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')



%% IMAGEM A CORES

indicescor=0;
for i=1:size(pclimpa.Location(:,1))
   indicescor= [indicescor find(ptCloudRot.Location(:,:,1)==pclimpa.Location(i,1) & ptCloudRot.Location(:,:,2)==pclimpa.Location(i,2) & ptCloudRot.Location(:,:,3)==pclimpa.Location(i,3))];
end

% LINHAS E COLUNAS DA MATRIZ DO OBJETO SELECIONADO
[row_c,col_c] = ind2sub([480 640],indicescor(2:end));

% criarIMAGEM preta
Black_roi=zeros(size(RGBFrameFlipped));

for i=1:size(col_c,2)
    Black_roi(row_c(i),col_c(i),:)=1;

    % qq(row(i),col(i),:)=RGBFrameFlipped
end


% mascara usada para ir buscar as partes coincidentes
RGBFrameFlipped(Black_roi==0)=0;
% guardar com um novo nome as variaveis
f_p_b_c=RGBFrameFlipped;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bnovaa = imbinarize(im2gray(f_p_b_c));
figure
imshow(bnovaa)

fill1 = imfill(bnovaa,'holes');
figure
imshow(fill1)

per1 = bwperim(fill1);
figure
imshow(per1)


[rowbw1,colbw1] = find(per1==1);

rowmax12 = max(rowbw1)+15;
rowmin12 = min(rowbw1)-15;
colmax12 = max(colbw1)+15;
colmin12 = min(colbw1)-15;

if rowmin12 < 0
    rowmin12 = 1;
end
if rowmax12 > size(per1,1)
    rowmax12 = size(per1,1);
end

if colmin12 < 0
    colmin12 = 1;
end
if colmax12 > size(per1,2)
    colmax12 = size(per1,2);
end


novaimagembw1 = per1(rowmin12:rowmax12,colmin12:colmax12,:);
figure
imshow(novaimagembw1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%





% 
% rowmax_c = max(max(row_c))+10;
% rowmin_c = min(min(row_c))-10;
% colmax_c = max(max(col_c))+10;
% colmin_c = min(min(col_c))-10;
% 
% % IMPEDIR QUE IMAGEM XMIN SEJA NEGATIVO E XMAX NAO ULTRAPASSE OS 640
% if rowmin_c < 0
%     rowmin_c = 1;
% elseif rowmax_c > 480
%     rowmax_c = 480;
% end
% 
% % IMPEDIR QUE IMAGEM YMIN SEJA NEGATIVO E YMAX NAO ULTRAPASSE OS 640
% if colmin_c < 0
%     colmin_c = 1;
% elseif colmax_c > 640
%     colmax_c = 640;
% end
% 
% f_p_b_c = f_p_b_c(rowmin_c:rowmax_c,colmin_c:colmax_c,:);
% figure
% imshow(f_p_b_c)



%% ROI AREA 
% figure
% imshow(f_p_b_c)
% 
% 
% h = imrect;
% position = wait(h);
% 
% croppedImage = imcrop(f_p_b_c, position);
% 
% figure;
% imshow(croppedImage);




% rgb cropped roi
figure
imshow(f_p_b_c)


h1 = imrect;
position1 = wait(h1);

croppedImage1 = imcrop(f_p_b_c, position1);

figure;
imshow(croppedImage1);



% depth cropped roi
% figure
% imshow(DEPTHFrameFlipped,[])
position2 = position1;
croppedImage2 =  imcrop(DEPTHFrameFlipped, position2);

figure;
imshow(croppedImage2,[]);



% % ptCloudOriginal2 = pcfromkinect(DEPTHDisp, croppedImage2);
% depthcropped = flip(croppedImage2,2);
% rgbcropped = flip(croppedImage1,2);
% 
% min(min(depthcropped(depthcropped> 0)))
% % nuvem nova
% 
% pcloud1 = pcfromkinect(DEPTHDisp, depthcropped, rgbcropped);



profmax=1700;
[pcloud, distance] = depthToCloud(croppedImage2,position1,profmax);


figure
pcshow(pcloud,"VerticalAxis","Y",VerticalAxisDir="Down")
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

xcloud = pcloud(:,:,1);
ycloud = pcloud(:,:,2);

figure(500)
plot(xcloud,ycloud,'.k')
set(gca,'ydir','reverse')
axis off

f= frame2im(getframe(figure(500)));
% I = imresize(f,[480,640]);
% figure
% imshow(f)
% title('frame do plot')

imagembin = imbinarize(im2gray(f));
inverseGrayImage = uint8(255) - im2uint8(imagembin);
% figure
% imshow(inverseGrayImage)
% title('frame binarizado')

se = strel('disk',10);
j = imclose(inverseGrayImage,se);
% figure
% imshow(j)


imedge = bwperim(j);
figure
imshow(imedge)


















bw3 = imdilate(inverseGrayImage, [1 1 1; 1 1 1; 1 1 1]);
figure
imshow(bw3)


bw4 =imdilate(bw3, [0 1 0; 1 1 1; 0 1 0]);
figure
imshow(bw4)

se = strel('sphere',9);
j = imclose(bw3,se);
figure
imshow(j)


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

fill = imfill(bi,'holes');
figure
imshow(fill)

per = bwperim(fill);
figure
imshow(per)


[rowbw,colbw] = find(per==1);

rowmax1 = max(rowbw)+15;
rowmin1 = min(rowbw)-15;
colmax1 = max(colbw)+15;
colmin1 = min(colbw)-15;

if rowmin1 < 0
    rowmin1 = 1;
end
if rowmax1 > size(per,1)
    rowmax1 = size(per,1);
end

if colmin1 < 0
    colmin1 = 1;
end
if colmax1 > size(per,2)
    colmax1 = size(per,2);
end


novaimagembw = per(rowmin1:rowmax1,colmin1:colmax1,:);
figure
imshow(novaimagembw)





reshape(A,1,[])













% localizacao
xnuvem = double(reshape(ptCloudRot.Location(:,:,1),[],1));
ynuvem = double(reshape(ptCloudRot.Location(:,:,2),[],1));
znuvem = double(reshape(ptCloudRot.Location(:,:,3),[],1));

% cores
ColorR = reshape(ptCloudRot.Color(:,:,1),[],1);
ColorB = reshape(ptCloudRot.Color(:,:,2),[],1);
ColorG = reshape(ptCloudRot.Color(:,:,2),[],1);
C = double([ColorR ColorB ColorG])/256;


figure
scatter3(xnuvem,znuvem,ynuvem,1,C);
xlabel('X(m)')
ylabel('Z(m)')
zlabel('Y(m)')
ax = gca();
ax.ZDir = 'reverse';
grid off