clear all
close all
clc

% %% LIGAR CAMARAS
% % LIGAR CAMARA RGB
% RGBDisp = videoinput('kinect',1,'RGB_640x480');
% 
% % LIGAR CAMARA DEPTH
% DEPTHDisp = videoinput('kinect',2,'Depth_640x480');
% 
% preview(DEPTHDisp)
% pause(20)
% 
% RGBFrameFlipped = flip(getsnapshot(RGBDisp),2);
% DEPTHFrameFlipped = flip(getsnapshot(DEPTHDisp),2);
% 
% %% NUVEM DE PONTOS COMPLETA
% % EXTRAIR A NUVEM DE PONTOS
% RGBFrame1 = getsnapshot(RGBDisp);
% DEPTHFrame1 = getsnapshot(DEPTHDisp);
% 
% ptCloudOriginal = pcfromkinect(DEPTHDisp, DEPTHFrame1, RGBFrame1);
% 
% 
% figure
% pcshow(ptCloudOriginal)
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')


load("Workspace_App.mat")

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

% MOSTAR NUVEM DE PONTOS
PointCloudBrowser = figure("Name",'Point Cloud Browser','NumberTitle','off');
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1]);
title('Full Point Cloud')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')



figure("Name",'Point Cloud Browser','NumberTitle','off');
pcshow(ptCloudRot,"VerticalAxis","Y",BackgroundColor=[1 1 1]);
% title('Full Point Cloud')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')


%% DEFINIR UMA ROI CUBICA PARA ASSUMIR A TARGET AREA
% ESPECIFICAR O STEP SIZE
stepSize = 100;

% AMOSTRA A NUVEM DE PONTOS DE ENTRADA E ARMAZENA AS COORDENADAS DE PONTOS COMO UM OBJETO DE NUVEM DE PONTOS
indices = 1:stepSize:ptCloudRot.Count;
tempPtCloud = select(ptCloudRot,indices);

% REMOVER OS PONTOS INVALIDOS DENTRO DA AMOSTRA ANTERIOR
[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);

% OBTER AS COORDENADAS DE LOCALIZAÇAO 3D DOS PONTOS DA NUVEM DE PONTOS
worldPoints = tempPtCloud.Location;

% CONVERTER E ENCONTRAR AS COORDENADAS DOS PONTOS 2D CORRESPONDENTES AOS PONTOS 3D DA NUVEM
[Y,X] = ind2sub([size(ptCloudRot.Location,1),size(ptCloudRot.Location,2)],indices);
imagePoints = [X(validIndices)' Y(validIndices)'];

% ESTIMATIVA DA MATRIZ DE PROJEÇAO DA CAMARA A PARTIR DA IMAGEM E DAS COORDENADAS DOS PONTOS DE LOCALIZAÇAO 3D
camMatrix = estimateCameraMatrix(imagePoints,worldPoints);

% DEFINIR A ROI CUBICA PARA A TARGET AREA
roi = [-0.29 0.13 0.2 0.415 1.2 1.7];
% roi = [-0.3 0.125 0 0.19 1.84 2.2];

% ENCONTRAR OS INDICES DOS PONTOS DA NUVEM QUE ESTAO DENTRO DA ROI CUBICA
indices = findPointsInROI(ptCloudRot,roi,camMatrix);

% SELECIONAR OS PONTOS DA NUVEM QUE SE ENCONTRAM DENTRO DA ROI
ptcloudB = select(ptCloudRot,indices);

% MOSTRAR A NUVEM DE PONTOS COM A TARGER AREA
figure(PointCloudBrowser);
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
pcshow(ptcloudB.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")
title('Full Point Cloud with Target Area')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% DEFINIÇÃO DOS PONTOS DOS VERTICES DO CUBO DA ROI
p1 = [roi(1) roi(4) roi(6)]';
p2 = [roi(1) roi(4) roi(5)]';
p3 = [roi(2) roi(4) roi(5)]';
p4 = [roi(2) roi(4) roi(6)]';
p5 = [roi(1) roi(3) roi(6)]';
p6 = [roi(1) roi(3) roi(5)]';
p7 = [roi(2) roi(3) roi(5)]';
p8 = [roi(2) roi(3) roi(6)]';

% DEFINIR AS FACES DO CUBO ATRAVES DOS VERTICES ANTERIORES
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];

% CORES DAS FACES
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 E MOSTRAR OBJETO
% DEFINIR A DISTANCIA MAXIMA PONTO-PLANO PARA A MONTAGEM DO PLANO
maxDistance = 0.0005;     % (m)

% DEFINIR O VETOR NORMAL AO PLANO A DEFINIR (NESTE CASO VETOR PREPENDICULAR AO PLANO Y)
referenceVector = [0,1,0];

% DETETAR O PLANO E EXTRAI-LO DA NUVEM DE PONTOS
[~,~,outlierIndices] = pcfitplane(ptcloudB,maxDistance,referenceVector);
remainPtCloud = select(ptcloudB,outlierIndices);

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

% MOSTRAR A NUVEM DE PONTOS LIMPA E SEM O PLANO DO CHAO
figure("Name",'Object Cloud Viewer','NumberTitle','off');
pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

%% CONTORNOS
% RETIRAR AS COORDENADAS DOS PONTOS DO OBJETO
xnuvem = pclimpa.Location(:,1);
ynuvem = pclimpa.Location(:,2);

% FAZER PLOT DOS PONTOS X E Y DO OBJETO E RODAR O PLOT
figure(100);
plot(xnuvem,ynuvem,'k.')
set(gca,'ydir','reverse')
axis 'off'

% RETIRAR FRAME DO PLOT ANTERIOR
% FramePlot = frame2im(getframe(figure(100)));
FramePlot = getframe(figure(100));
imgFramePlot = FramePlot.cdata;

figure;
imshow(imgFramePlot)

% BINARIZAR IMG E INVERTER COR
ImgBin = imbinarize(im2gray(imgFramePlot));
InverseImg = uint8(255) - im2uint8(ImgBin);
figure
imshow(ImgBin)
figure
imshow(InverseImg)



% IMAGE BINARIZATION AND COLOUR INVERSION
ImgBin = imbinarize(im2gray(imgFramePlot));
InverseImg = uint8(255) - im2uint8(ImgBin);


% MORPHOLOGICAL CLOSING USING DISK AS STRUCTURING ELEMENT
se = strel('disk',strel_radius);
ImgClosed = imclose(InverseImg,se);


% PREENCHER ESPAÇOS ABERTOS NO OBJETO
raio_strel = 8;
se = strel('disk',raio_strel);
% ImgClosed = imclose(InverseImg,se);
figure
imshow(ImgClosed)

% FIND CONTOURS
ImgEdge = edge(ImgClosed,"canny");

% FAZER CONTORNOS DO OBJETO
% ImgEdge = edge(ImgClosed,"canny");
figure
imshow(ImgEdge)

% INVERTER A IMAGEM BINARIA PARA AS COORDENADAS DOS CONTORNOS NÃO ESTAREM INVERTIDOS (0,0 EM IMAGENS COMEÇA CANTO SUPERIOR ESQUERDO)
imgflip = flip(ImgClosed);
figure
imshow(imgflip)

%% RETIRAR E ALINHAR CONTORNOS
% RETIRAR COORDENADAS x y E z DOS PONTOS DA NUVEM DO OBJETO
xnuvem = pclimpa.Location(:,1);
ynuvem = pclimpa.Location(:,2);
znuvem = pclimpa.Location(:,3);

% CENTRAR OS PONTOS COM A ORIGEM DO REFERENCIAL
if min(xnuvem)<0
    xnuvemnova = xnuvem+abs(min(xnuvem));
else
    xnuvemnova = xnuvem-min(xnuvem);
end

ynuvemnova = abs(ynuvem-abs(max(ynuvem)));

bwflip = bwboundaries(imgflip);
figure
hold on
for i=1:size(bwflip,1)
    if i==1
        bw1flip = bwflip{i};
        bw1flip = [bw1flip(:,2),(bw1flip(:,1))];

        % FATOR DE AMPLIAÇÃO DA MATRIZ DOS CONTORNOS PARA COINCIDIR COM A MATRIZ DOS PONTOS DA NUVEM
        fator_correct_x = (max(bw1flip(:,1)-min(bw1flip(:,1))))/max(xnuvemnova);
        fator_correct_y = (max(bw1flip(:,2)-min(bw1flip(:,2))))/max(ynuvemnova);

        % FAZER PLOT DOS CONTORNOS EXTERNOS E ALINHA-LOS COM A ORIGEM DO REFERENCIAL
        plot((bw1flip(:,1)-min(bw1flip(:,1)))/fator_correct_x,((bw1flip(:,2))-min(bw1flip(:,2)))/fator_correct_y,'r','LineWidth',2);

    elseif i>1
        bw2flip=bwflip{i};
        bw2flip = [bw2flip(:,2),(bw2flip(:,1))];

        % FAZER PLOT DOS CONTORNOS INTERNOS E ALINHA-LOS COM A ORIGEM DO REFERENCIAL
        plot((bw2flip(:,1)-min(bw1flip(:,1)))/fator_correct_x,(bw2flip(:,2)-min(bw1flip(:,2)))/fator_correct_y,'LineWidth',2);
    end
end

% PLOT DOS PONTOS DA NUVEM CENTRADA
plot(xnuvemnova,ynuvemnova,'k.')
title('Object Contours')
hold off

%% RETIRAR DADOS x E y DE TODOS OS CONTORNOS
ax = gca; 
h = findobj(gca,'Type','line'); 

% CONTORNO EXTERNO
h1 = h(end,:);
xdata = h(end,:).XData;
ydata = h(end,:).YData;
contour_ext = [xdata;ydata]';

%% TENTAR RETIRAR PONTOS DE FRONTEIRA
% FRONTEIRA EXTERIOR
P = [xnuvemnova ynuvemnova];
PQ_ext = contour_ext;
[k_ext,~] = dsearchn(P,PQ_ext);

figure
plot(P(:,1),P(:,2),'ko')
hold on
plot(PQ_ext(:,1),PQ_ext(:,2),'-g','LineWidth',3)
hold on
plot(P(k_ext,1),P(k_ext,2),'*r','MarkerSize',4)
hold on

%% FRONTEIRA(S) INTERIOR(S)
% CONTORNOS INTERNOS
if size(bwflip,1) >= 2
    for i=2:size(bwflip,1)
        contour_int{1,i-1} = {[h(i,:).XData' h(i,:).YData']};
    end

    % figure
    k_int = mat2cell(repmat(zeros(0,1),1,size(contour_int,2)),0,ones(1,size(contour_int,2)));
    for i=1:size(contour_int,2)
        hold on
        cont_int = cell2mat(contour_int{i});
        plot(cont_int(:,1),cont_int(:,2),'-g','LineWidth',3)


        PQ_int = contour_int;
        k_int{1,i} = dsearchn(P,cell2mat(PQ_int{i}));

        hold on
        plot(P(k_int{i},1),P(k_int{i},2),'*r','MarkerSize',4)
        legend('Data Points','Contours','Nearest Points','Location','nw')
    end
    for i=1:size(k_int,2)
        if i==1
            pontos_contorno = [k_ext;k_int{i}];
        else
            pontos_contorno = [pontos_contorno;k_int{i}];
        end
    end
else
    pontos_contorno = k_ext;
end

pontos_contorno = pclimpa.Location(pontos_contorno,:);

figure
plot3(pontos_contorno(:,1),pontos_contorno(:,3),pontos_contorno(:,2),'k.')
set(gca,'zdir','reverse')
title('Contour Points')
xlabel('X(m)')
ylabel('Z(m)')
zlabel('Y(m)')
xlim([min(pontos_contorno(:,1))-0.005 max(pontos_contorno(:,1))+0.005])
ylim([min(pontos_contorno(:,3))-0.005 max(pontos_contorno(:,3))+0.005])
zlim([min(pontos_contorno(:,2))-0.005 max(pontos_contorno(:,2))+0.005])
grid on