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

% % 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(xnuvem);
% max_y_malha = abs(min(ynuvem));
% 
% % 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);

% MOSTRAR CONTORNOS SOBREPOSTOS COM A MESH
% figure
% plot(x2,y2,'k.','MarkerSize',5);
% xlim([min(x2) max(x2)])
% ylim([min(y2) max(y2)])

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


% figure
% real_contour = plot(x2,y2,'k.','MarkerSize',5);
% xlim([min(x2)-5 max(x2)+5])
% ylim([min(y2)-5 max(y2)+5])


%% CRIAR A MESH A PARTIR DA TRIANGULAÇAO DE DELAUNAY

DT = delaunayTriangulation(xmeshnova,ymeshnova);
% DT(1,:)
% 
% figure
% triplot(DT)
% hold on
% plot(DT(1,:),DT(2,:),'*r')
% 
% dts = struct('Points', DT.Points, 'ConnectivityList', DT.ConnectivityList);
% 
% dts.ConnectivityList(760) = [];
% 
% figure
% trisurf(dts.ConnectivityList, dts.Points);


% DT.ConnectivityList
%% 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')
% hold on
% plot(xmeshnova,ymeshnova,'or','MarkerSize',4)

% 3D DELAUNAY MESH OBJETO
% figure(500)
figure
hidden on
trimesh(DT.ConnectivityList,xmesh,zmesh,ymesh)
xlabel('x')
ylabel('z')
zlabel('y')
ax = gca;
ax.ZDir = 'reverse';
title('3D Delaunay Mesh')
xlim([min(xmesh) max(xmesh)])
ylim([min(zmesh) max(zmesh)])
zlim([min(ymesh) max(ymesh)])




NTriangles = size(DT.ConnectivityList,1);

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


% triangulos com areas muito pequenas ou muito grandes
tri_large = find(Areas > 12);
tri_small = find(Areas < 0.5);

% matriz de todos os triangulos a remover
triangulos_a_remover = [tri_large(:,1);tri_small(:,1)];

% converter a delaunay triangulation para estutura para se conseguir "manipular" os dados
dts = struct('Points', DT.Points, 'ConnectivityList', DT.ConnectivityList);

% remover a conectividade dos triangulos a remover
dts.ConnectivityList(triangulos_a_remover,:) = [];

% fazer reshape da nova connectivity list
nova_connectivityList = reshape(dts.ConnectivityList,[],3);

% visualizar a nova delaunay triangulation
figure
triplot(nova_connectivityList,dts.Points(:,1),dts.Points(:,2))
xlabel('x')
ylabel('y')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(ymeshnova)-5 max(ymeshnova)+5])


%% mesh 3D sem triangulos maiores/pequenos
figure
hidden on
trimesh(nova_connectivityList,xmesh,zmesh,ymesh)
xlabel('x')
ylabel('z')
zlabel('y')
ax = gca;
ax.ZDir = 'reverse';
title('3D Delaunay Mesh')
xlim([min(xmesh) max(xmesh)])
ylim([min(zmesh) max(zmesh)])
zlim([min(ymesh) max(ymesh)])





%% CALCULAR DISTANCIAS ENTRE DT CONNECTIVITY LIST


% dists = [xmeshnova,ymeshnova];
% dists_pontos = reshape(pdist(dists,"euclidean"),[],1);
% mean(dists_pontos)



V1 = [xmeshnova(dts.ConnectivityList(1,1)),ymeshnova(dts.ConnectivityList(1,1))];
V2 = [xmeshnova(dts.ConnectivityList(1,2)),ymeshnova(dts.ConnectivityList(1,2))];
V3 = [xmeshnova(dts.ConnectivityList(1,3)),ymeshnova(dts.ConnectivityList(1,3))];



figure(69)
triplot(nova_connectivityList,dts.Points(:,1),dts.Points(:,2))
xlabel('x')
ylabel('y')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(ymeshnova)-5 max(ymeshnova)+5])
title('2D Delaunay Mesh')
hold on
plot(V1(:,1),V1(:,2),'*r')
hold on
plot(V2(:,1),V2(:,2),'*g')
hold on
plot(V3(:,1),V3(:,2),'*k')




% CALCULAR (x,y) DOS VERTICES DA CONNECTIVITY LIST
NVertices = size(dts.ConnectivityList,1);
matrix_vertices_coordenadas = zeros([size(dts.ConnectivityList,1) 6]);
for i=1:NVertices
    matrix_vertices_coordenadas(i,:) = [xmeshnova(dts.ConnectivityList(i,1)) ymeshnova(dts.ConnectivityList(i,1)) ...
        xmeshnova(dts.ConnectivityList(i,2)) ymeshnova(dts.ConnectivityList(i,2)) ...
        xmeshnova(dts.ConnectivityList(i,3)) ymeshnova(dts.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)];


% CALCULAR AS DISTANCIAS ENTRE VERTICES
distancias_arestas = zeros([size(dts.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


% media_dists = mean([distancias_arestas(:,1);distancias_arestas(:,2);distancias_arestas(:,3)]);


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

% CONVERTER EM ESTRUTURA PARA FAZER MANIPULAÇAO DE DADOS
dts1 = struct('Points', dts.Points, 'ConnectivityList', dts.ConnectivityList);



for i=1:size(arestas_grandes_row,1)
% REMOVER AS ARESTAS MAIORES
dts1.ConnectivityList(arestas_grandes_row(i,:)) = [];
end





% FAZER RESHAPE DA MATRIZ ANTERIOR PARA FAZER O PLOT
% nova_connectivityList1 = reshape(dts1.ConnectivityList,[],3);


% visualizar a nova delaunay triangulation
figure
triplot(dts1.ConnectivityList,dts1.Points(:,1),dts1.Points(:,2))
xlabel('x')
ylabel('y')
xlim([min(xmeshnova)-5 max(xmeshnova)+5])
ylim([min(ymeshnova)-5 max(ymeshnova)+5])








% %% CONSTRAIN MESH
% % NOVOS CONTORNOS
% [B,L,N,A] = bwboundaries(ImgClosed); 
% figure
% imshow(ImgClosed)
% hold on; 
% for k = 1:N
%     % Boundary k is the parent of a hole if the k-th column of the adjacency matrix A contains a non-zero element
%     if (nnz(A(:,k)) > 0)
%         boundary = B{k};
%         redline = plot(boundary(:,2),boundary(:,1),'r','LineWidth',2);
%         % Loop through the children of boundary k
%         for l = find(A(:,k))'
%             boundary = B{l};
%             greenline(l,:) = plot(boundary(:,2),boundary(:,1),'g','LineWidth',2);
%         end
%     end
% end
% 
% 
% 
% figure
% axes
% plot(redline.XData,redline.YData,'r','LineWidth',2)
% set(gca,'ydir','reverse')
% hold on
% x=redline.XData;
% y=redline.YData;
% c = [(1:length(x)-1).' (2:length(x)).'];
% for jj = 1:size(greenline,1)
%     try
%         plot(greenline(jj).XData,greenline(jj).YData,'g','LineWidth',2);
%         x_start = length(x);
%         x=[x greenline(jj).XData];
%         y=[y greenline(jj).YData];
%         c = [c;(x_start+(1:length(greenline(jj).XData)-1)).' (x_start+(2:length(greenline(jj).XData))).'];
%     catch
%     end
% end










% DT = delaunayTriangulation(xmeshnova,ymeshnova,c);
% 
% % IC = incenter(DT);
% figure
% triplot(DT)
% 
% 
% 
% figure
% IO = isInterior(DT);
% triplot(DT(IO, :),DT.Points(:,1), DT.Points(:,2),'LineWidth', 2)
% set(gca,'ydir','reverse')









% 
% DT = delaunayTriangulation(xmeshnova,ymeshnova,c);
% 
% 
% 
% 
% IO = isInterior(DT);

% 
% DT = delaunayTriangulation(x',y',c);
% 
% IO = isInterior(DT);
% triplot(DT(IO, :),DT.Points(:,1), DT.Points(:,2),'LineWidth', 2)
% 
% figure
% h=trimesh(DT(IO, :),DT.Points(:,1), DT.Points(:,2),zeros(size(DT.Points(:,2))),'LineWidth', 2);
% 
% [Fs,Vs]=subtri(h.Faces,h.Vertices,6);
% % Note, you can increase the density by increase the value of the last input
% 
% figure
% patch('Faces',h.Faces,'Vertices',h.Vertices,'FaceAlpha',0.5,'EdgeColor','k','LineWidth',2);
% set(gca,'ydir','reverse')
% 
% figure
% patch('Faces',Fs,'Vertices',Vs,'FaceAlpha',0.01,'EdgeColor','k','LineWidth',1);
% set(gca,'ydir','reverse')
% 












































% descriptors.tri = pointLocation(DT,xmeshnova,ymeshnova);
% descriptors.baryCoords = cartesianToBarycentric(DT,descriptors.tri,[xmeshnova ymeshnova]);


% cc1 = [210 90];
% circ1 = (143:180)';
% x(circ1) = (x(circ1)-cc1(1))*0.6 + cc1(1);
% y(circ1) = (y(circ1)-cc1(2))*0.6 + cc1(2);
% tr = triangulation(DT(:,:),x',y');
% clf
% figure
% triplot(tr)


% Xnew = barycentricToCartesian(tr,descriptors.tri,descriptors.baryCoords);
% tr = triangulation(DT,Xnew);
% clf
% triplot(tr)
% axis([-10 310 -10 310])
% axis equal
% xlabel('Morphed Mesh','FontWeight','b')