clear all
close all
clc

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");
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.35 0.15 0.17 0.415 1.1 1.6];

% 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",50);

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

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

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

%% MESH
% RETIRAR AS COORDENADAS DOS PONTOS DO OBJETO
xmesh = double(pclimpa.Location(:,1));
ymesh = double(pclimpa.Location(:,2));
zmesh = double(pclimpa.Location(:,3));

% MULTIPLICAR OS PONTOS POR 1000 PARA OBTER A MESMA ORDEM DE GRANDEZA
matrixmesh = [xmesh ymesh]*1000;

% CENTRAR A MESH COM A ORIGEM DO REFERENCIAL (0,0)
xmeshnova = matrixmesh(:,1)+abs(min(matrixmesh(:,1)));
ymeshnova = abs(matrixmesh(:,2)-abs(max(matrixmesh(:,2))));


%% INDICES DOS PONTOS DOS CONTORNOS
[y,x] = find(ImgEdge); 

% CENTRAR OS CONTORNOS COM A ORIGEM DO REFERENCIAL (0,0)
x1 = x-abs(min(x));
y1 = y-abs(max(y));

% MAX MALHA
max_x_malha = max(xmeshnova);
max_y_malha = max(ymeshnova);

% MAX CONTORNOS
max_x_contorno = max(x1);
max_y_contorno = abs(min(y1));

% FATOR DE MULTIPLICAÇAO DAS MATRIZES ANTERIORES
% X
fator_x = max_x_malha/max_x_contorno;

% Y
fator_y = max_y_malha/max_y_contorno;

% MATRIZ DOS CONTORNOS DO TAMANHO DA MESH
x2 = x1*fator_x;
y2 = abs(y1*fator_y);

%% CRIAR A MESH A PARTIR DA TRIANGULAÇAO DE DELAUNAY
DT = delaunayTriangulation(xmeshnova,ymeshnova);

%% DELAUNAY MESH'S
% 2D DELAUNAY MESH OBJETO
figure;
triplot(DT);
xlabel('x')
ylabel('y')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(ymeshnova)-5 max(ymeshnova)+5])
title('2D Delaunay Mesh')

% 3D DELAUNAY MESH OBJETO
% figure(500)
figure
hidden on
trimesh(DT.ConnectivityList,xmeshnova,zmesh,ymeshnova)
xlabel('x')
ylabel('z')
zlabel('y')
title('3D Delaunay Mesh')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(zmesh) max(zmesh)])
zlim([min(ymeshnova)-5 max(ymeshnova)+5])



%% AREAS
NVertices = size(DT.ConnectivityList,1);
% calcular as areas dos triangulos da DT
Areas = zeros(NVertices,1);
for i = 1:NVertices
    PointIndexes = DT.ConnectivityList(i,:);
    Areas(i) = polyarea(DT.Points(PointIndexes,1),DT.Points(PointIndexes,2));
end


%% VERTICES EM COORDENADAS
% CALCULAR (x,y) DOS VERTICES DA CONNECTIVITY LIST
% NVertices = size(DT.ConnectivityList,1);
matrix_vertices_coordenadas = zeros([size(DT.ConnectivityList,1) 6]);
for i=1:NVertices
    matrix_vertices_coordenadas(i,:) = [xmeshnova(DT.ConnectivityList(i,1)) ymeshnova(DT.ConnectivityList(i,1)) ...
        xmeshnova(DT.ConnectivityList(i,2)) ymeshnova(DT.ConnectivityList(i,2)) ...
        xmeshnova(DT.ConnectivityList(i,3)) ymeshnova(DT.ConnectivityList(i,3))];
end

% TRES MATRIZES COM OS (x,y) DOS VERTICES DE CADA TRIANGULO
primeiro_vertice_coord = [matrix_vertices_coordenadas(:,1) matrix_vertices_coordenadas(:,2)];
segundo_vertice_coord = [matrix_vertices_coordenadas(:,3) matrix_vertices_coordenadas(:,4)];
terceiro_vertice_coord = [matrix_vertices_coordenadas(:,5) matrix_vertices_coordenadas(:,6)];


%% COMPRIMENTO DAS ARESTAS
% CALCULAR AS DISTANCIAS ENTRE VERTICES
distancias_arestas = zeros([size(DT.ConnectivityList,1) 3]);
for i=1:NVertices
    distancias_arestas(i,:) = [norm(primeiro_vertice_coord(i,:)-segundo_vertice_coord(i,:))...
        norm(primeiro_vertice_coord(i,:)-terceiro_vertice_coord(i,:)) ...
        norm(segundo_vertice_coord(i,:)-terceiro_vertice_coord(i,:))];
end

%% ARESTAS GRANDES E AREAS GRANDES
% ROW E COL (INDEX) DAS ARESTAS GRANDES
[arestas_grandes_row, arestas_grandes_col] = find(distancias_arestas(:,:) > 10);

% triangulos com areas muito pequenas ou muito grandes
areas_grandes = find(Areas > 10);

% INDICES DOS TRIANGULOS COM AREAS E ARESTAS GRANDES
triangulos_remove = [arestas_grandes_row;areas_grandes];

%% NOVA CONNECTIVITY LIST
NConnectivity = size(DT.ConnectivityList,1);
nova_connectivityList2 = zeros(size(DT.ConnectivityList,1),3);
for i=1:NConnectivity
    if i ~= triangulos_remove   
        nova_connectivityList2(i,:) = [DT.ConnectivityList(i,:)];
    end
end

nova_connectivityList2(~any(nova_connectivityList2,2),:) = [];

%% MESH 2D
figure
triplot(nova_connectivityList2,DT.Points(:,1),DT.Points(:,2))
xlabel('x')
ylabel('y')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(ymeshnova)-5 max(ymeshnova)+5])
title('2D Constrained Delaunay Triangulation')
hold on
plot(x2,y2,'.k','MarkerSize',4)

%% MESH 3D
figure
trimesh(nova_connectivityList2,xmeshnova,zmesh,ymeshnova)
xlabel('x')
ylabel('z')
zlabel('y')
title('3D Constrained Delaunay Mesh')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(zmesh) max(zmesh)])
zlim([min(ymeshnova)-5 max(ymeshnova)+5])



% DT_3d = delaunayTriangulation(xmeshnova,zmesh,ymeshnova);
% 
% 
% 
% ConnectivityList3D = reshape(DT_3d.ConnectivityList,[],3);
% 
% 
% %% AREAS
% NVertices_3d = size(ConnectivityList3D,1);
% % calcular as areas dos triangulos da DT
% Areas_3d = zeros(NVertices_3d,1);
% for i = 1:NVertices_3d
%     PointIndexes_3d = ConnectivityList3D(i,:);
%     Areas_3d(i) = polyarea(DT_3d.Points(PointIndexes_3d,1),DT_3d.Points(PointIndexes_3d,2));
% end
% 
% 
% %% VERTICES EM COORDENADAS
% % CALCULAR (x,y) DOS VERTICES DA CONNECTIVITY LIST
% % NVertices = size(DT.ConnectivityList,1);
% matrix_vertices_coordenadas_3d = zeros([size(ConnectivityList3D,1) 6]);
% for i=1:NVertices_3d
%     matrix_vertices_coordenadas_3d(i,:) = [xmeshnova(ConnectivityList3D(i,1)) ymeshnova(ConnectivityList3D(i,1)) ...
%         xmeshnova(ConnectivityList3D(i,2)) ymeshnova(ConnectivityList3D(i,2)) ...
%         xmeshnova(ConnectivityList3D(i,3)) ymeshnova(ConnectivityList3D(i,3))];
% end
% 
% % TRES MATRIZES COM OS (x,y) DOS VERTICES DE CADA TRIANGULO
% primeiro_vertice_coord_3d = [matrix_vertices_coordenadas_3d(:,1) matrix_vertices_coordenadas_3d(:,2)];
% segundo_vertice_coord_3d = [matrix_vertices_coordenadas_3d(:,3) matrix_vertices_coordenadas_3d(:,4)];
% terceiro_vertice_coord_3d = [matrix_vertices_coordenadas_3d(:,5) matrix_vertices_coordenadas_3d(:,6)];
% 
% %% COMPRIMENTO DAS ARESTAS
% % CALCULAR AS DISTANCIAS ENTRE VERTICES
% distancias_arestas_3d = zeros([size(ConnectivityList3D,1) 3]);
% for i=1:NVertices_3d
%     distancias_arestas_3d(i,:) = [norm(primeiro_vertice_coord_3d(i,:)-segundo_vertice_coord_3d(i,:))...
%         norm(primeiro_vertice_coord_3d(i,:)-terceiro_vertice_coord_3d(i,:)) ...
%         norm(segundo_vertice_coord_3d(i,:)-terceiro_vertice_coord_3d(i,:))];
% end
% 
% %% ARESTAS GRANDES E AREAS GRANDES
% % ROW E COL (INDEX) DAS ARESTAS GRANDES
% [arestas_grandes_row_3d, arestas_grandes_col_3d] = find(distancias_arestas_3d(:,:) > 80);
% 
% % triangulos com areas muito pequenas ou muito grandes
% areas_grandes_3d = find(Areas_3d > 20);
% 
% % INDICES DOS TRIANGULOS COM AREAS E ARESTAS GRANDES
% triangulos_remove_3d = [arestas_grandes_row_3d;areas_grandes_3d];
% 
% %% NOVA CONNECTIVITY LIST
% NConnectivity_3d = size(ConnectivityList3D,1);
% nova_connectivityList2_3d = zeros(size(ConnectivityList3D,1),3);
% for i=1:NConnectivity_3d
%     if i ~= triangulos_remove_3d   
%         nova_connectivityList2_3d(i,:) = [ConnectivityList3D(i,:)];
%     end
% end
% 
% nova_connectivityList2_3d(~any(nova_connectivityList2_3d,2),:) = [];
% 
% 
% figure 
% trimesh(nova_connectivityList2_3d,xmeshnova,zmesh,ymeshnova)
% xlabel('x')
% ylabel('y')
% zlabel('z')