clc; clear ; close all

% CARREGAR WORKSPACE COM A NUVEM DO AMBIENTE CAPTURADA PELA KINECT
load("workspace_cloud.mat")
ptObject = pclimpa;
% NUVEM DE PONTOS DO OBJETO
figure("Name",'Object Cloud Viewer','NumberTitle','off');
pcshow(ptObject,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

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

% point = zeros(size(pclimpa.Location,1),3);
% % CRIAR CÉLULA PARA GUARDAR QUAIS OS PONTOS DE VIZINHANÇA ENCONTRADOS
% indices1 = mat2cell(repmat(zeros(0,1),1,size(pclimpa.Location,1)),0,ones(1,size(pclimpa.Location,1)));
% for i=1:size(pclimpa.Location,1)
%     % ENCONTRAR PONTOS VIZINHANÇA COM x radius
%     point(i,:) = pclimpa.Location(i,:);
%     radius = 0.0075;
%     [indices1{i},dists] = findNeighborsInRadius(pclimpa,point(i,:),radius);
% 
%     % QUANTIDADE DE PONTOS DE VIZINHANÇA
%     K = zeros(size(indices1,2),1);
%     for j=1:size(indices1,2)
%         K(j,:) = numel(indices1{j})-1;
% 
%         % NÃO DEIXAR QUE O NÚMERO DE VIZINHANÇA SEJA < 3
%         if K(j,:) < 3
%             K(j,:) = 3;
%         end
%     end
% end
% 
% % CALCULAR NORMAIS COM A MEDIA DE Nº PONTOS DE VIZINHANÇA
% normals = pcnormals(pclimpa,round(mean(K)));
% 
% % VETORES UNITARIOS
% u = normals(:,1);
% v = normals(:,2);
% w = normals(:,3);
% 
% % ALINHAR AS NORMAS COM O CENTRO DO REFERENCIAL AKA CENTRO DA KINECT
% sensorCenter = [0,0,0];
% for k = 1 : numel(xnuvem)
%     p1 = sensorCenter - [xnuvem(k),ynuvem(k),znuvem(k)];
%     p2 = [u(k),v(k),w(k)];
%     % Flip the normal vector if it is not pointing towards the sensor.
%     angle = atan2(norm(cross(p1,p2)),p1*p2');
%     if angle > pi/2 || angle < -pi/2
%         u(k) = -u(k);
%         v(k) = -v(k);
%         w(k) = -w(k);
%     end
% end

% % MOSTRAR NORMAS ALINHADAS
% figure
% pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
% hold on
% q = quiver3(xnuvem, ynuvem, znuvem, u, v, w);
% hold off
% title(['Adjusted Normals of Point Cloud k(mean) = ',num2str(round(mean(K))),' radius r = ',num2str(radius)])
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')

%% GRIDAVERAGE
gridStep = 0.04;

% CLOUD DOWNSAMPLING USING GRIDAVERAGE METHOD
ptGridAvgDownS = pcdownsample(ptObject,'gridAverage',gridStep);


figure
pcshow(ptGridAvgDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
% title(['Point Cloud GridAverage DownSampling with a 3D grid box filter size = ',num2str(gridStep)])
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% % GRIDAVERAGE LOCATION POINTS
% xGridAvgDownS = ptGridAvgDownS.Location(:,1);
% yGridAvgDownS = ptGridAvgDownS.Location(:,2);
% zGridAvgDownS = ptGridAvgDownS.Location(:,3);
% 
% % CALCULAR NORMAIS
% normalsGridAvgDownS = pcnormals(ptGridAvgDownS,10);
% 
% % VETORES UNITARIOS
% uGridAvgDownS = normalsGridAvgDownS(:,1);
% vGridAvgDownS = normalsGridAvgDownS(:,2);
% wGridAvgDownS = normalsGridAvgDownS(:,3);
% 
% % MOSTRAR NORMAS ALINHADAS
% figure
% pcshow(ptGridAvgDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
% hold on
% quiver3(xGridAvgDownS, yGridAvgDownS, zGridAvgDownS, uGridAvgDownS, vGridAvgDownS, wGridAvgDownS);
% hold off
% title('GridAverageSample Point Cloud pcNormals Normals')
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')

% % ALINHAR AS NORMAS COM O CENTRO DO REFERENCIAL AKA CENTRO DA KINECT
% sensorCenter = [0,0,0];
% for k = 1 : numel(xGridAvgDownS)
%     p1 = sensorCenter - [xGridAvgDownS(k),yGridAvgDownS(k),zGridAvgDownS(k)];
%     p2 = [uGridAvgDownS(k),vGridAvgDownS(k),wGridAvgDownS(k)];
%     % Flip the normal vector if it is not pointing towards the sensor.
%     angle = atan2(norm(cross(p1,p2)),p1*p2');
%     if angle > pi/2 || angle < -pi/2
%         uGridAvgDownS(k) = -uGridAvgDownS(k);
%         vGridAvgDownS(k) = -vGridAvgDownS(k);
%         wGridAvgDownS(k) = -wGridAvgDownS(k);
%     end
% end

% % MOSTRAR NORMAS ALINHADAS
% figure
% pcshow(ptGridAvgDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
% hold on
% quiver3(xGridAvgDownS, yGridAvgDownS, zGridAvgDownS, uGridAvgDownS, vGridAvgDownS, wGridAvgDownS);
% hold off
% title('GridAverageSample Point Cloud aligned pcNormals Normals')
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')

figure
subplot(3,2,1)
plot(ptGridAvgDownS.Location(:,1),ptGridAvgDownS.Location(:,2),'.k')
set(gca,'ydir','reverse')

% DESENHAR QUADRADO
h1 = drawrectangle;
wait(h1)

% RETIRAR COORDENADAS DOS PONTOS DENTRO DO QUADRADO
% QUADRADO
square_xmin = min(h1.Vertices(:,1));
square_xmax = max(h1.Vertices(:,1));
square_ymin = min(h1.Vertices(:,2));
square_ymax = max(h1.Vertices(:,2));

% PONTOS DENTRO DO QUADRADO
square_points = find(ptGridAvgDownS.Location(:,1)>square_xmin & ptGridAvgDownS.Location(:,1)<square_xmax & ...
                     ptGridAvgDownS.Location(:,2)>square_ymin & ptGridAvgDownS.Location(:,2)<square_ymax );
square_points = ptGridAvgDownS.Location(square_points,:);

% NUVEM RECORTADA
% figure
subplot(3,2,2)
plot3(square_points(:,1),square_points(:,3),square_points(:,2),'k.')
set(gca,'zdir','reverse')
axis tight
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')

% % MOSTRAR A PARTE RECORTADA
% figure;
% plot3(ptGridAvgDownS.Location(:,1),ptGridAvgDownS.Location(:,3),ptGridAvgDownS.Location(:,2),'k.')
% set(gca,'zdir','reverse')
% axis tight
% grid on
% hold on
% square_pntCropped = plot3(square_points(:,1),square_points(:,3),square_points(:,2),'g.');
% legend(square_pntCropped,'Cropped Part','Location','southeast')

%% ELIMINAR ZONAS
% figure
subplot(3,2,3)
plot(square_points(:,1),square_points(:,2),'.k')
set(gca,'ydir','reverse')

% DESENHAR QUADRADO
h2 = drawrectangle;
wait(h2)

% RETIRAR COORDENADAS DOS PONTOS DENTRO DO QUADRADO
% QUADRADO
square_xmin1 = min(h2.Vertices(:,1));
square_xmax1 = max(h2.Vertices(:,1));
square_ymin1 = min(h2.Vertices(:,2));
square_ymax1 = max(h2.Vertices(:,2));

% PONTOS DENTRO DO QUADRADO
square_pointsRemove = find(square_points(:,1)>square_xmin1 & square_points(:,1)<square_xmax1 & ...
                           square_points(:,2)>square_ymin1 & square_points(:,2)<square_ymax1 );
% PONTOS A RETIRAR
square_pointsRemove1 = square_points(square_pointsRemove,:);

% NUVEM RECORTADA
% figure
subplot(3,2,4)
plot3(square_pointsRemove1(:,1),square_pointsRemove1(:,3),square_pointsRemove1(:,2),'k.')
set(gca,'zdir','reverse')
axis tight
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')

% % MOSTRAR A PARTE RECORTADA
% figure
% plot3(square_points(:,1),square_points(:,3),square_points(:,2),'.k')
% set(gca,'zdir','reverse')
% axis tight
% grid on
% hold on
% square_pntElimin1 = plot3(square_pointsRemove1(:,1),square_pointsRemove1(:,3),square_pointsRemove1(:,2),'r.');
% legend(square_pntElimin1,'Eliminated Points','Location','southeast')


NPontosRemov = size(square_points,1);
pc_cPartesRemoved = zeros(size(square_points,1),3);
for i=1:NPontosRemov
    if i ~= square_pointsRemove   
        pc_cPartesRemoved(i,:) = square_points(i,:);
    end
end

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

% figure
subplot(3,2,5)
plot3(pc_cPartesRemoved(:,1),pc_cPartesRemoved(:,3),pc_cPartesRemoved(:,2),'k.')
set(gca,'zdir','reverse')
axis tight
grid on


% figure
subplot(3,2,6)
plot3(ptGridAvgDownS.Location(:,1),ptGridAvgDownS.Location(:,3),ptGridAvgDownS.Location(:,2),'k.')
set(gca,'zdir','reverse')
axis tight
grid on
hold on
square_pntCropped = plot3(square_points(:,1),square_points(:,3),square_points(:,2),'g.');
square_pntRemoved = plot3(square_pointsRemove1(:,1),square_pointsRemove1(:,3),square_pointsRemove1(:,2),'r.');
legend([square_pntCropped square_pntRemoved],{'Cropped Part' 'Removed Points'},'Location','southeast')









figure
plot3(pc_cPartesRemoved(:,1),pc_cPartesRemoved(:,3),pc_cPartesRemoved(:,2),'k.')
set(gca,'zdir','reverse')
axis tight
grid on

figure
plot(pc_cPartesRemoved(:,1),pc_cPartesRemoved(:,2),'k.')
set(gca,'ydir','reverse')
h3 = drawrectangle;
wait(h3)

% RETIRAR COORDENADAS DOS PONTOS DENTRO DO QUADRADO
% QUADRADO
square_xmin2 = min(h3.Vertices(:,1));
square_xmax2 = max(h3.Vertices(:,1));
square_ymin2 = min(h3.Vertices(:,2));
square_ymax2 = max(h3.Vertices(:,2));

% PONTOS DENTRO DO QUADRADO
square_pointsRemove11 = pc_cPartesRemoved(:,1)>square_xmin2 & pc_cPartesRemoved(:,1)<square_xmax2 & ...
                        pc_cPartesRemoved(:,2)>square_ymin2 & pc_cPartesRemoved(:,2)<square_ymax2;
% PONTOS A RETIRAR
square_pointsRemove11 = pc_cPartesRemoved(square_pointsRemove11,:);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xdens = ptGridAvgDownS.Location(:,1);
ydens = ptGridAvgDownS.Location(:,2);
zdens = ptGridAvgDownS.Location(:,3);

[xyzdens] = find( xdens > min(square_pointsRemove11(:,1)) & xdens < max(square_pointsRemove11(:,1)) & ...
                  ydens > min(square_pointsRemove11(:,2)) & ydens < max(square_pointsRemove11(:,2)) & ...
                  zdens > min(square_pointsRemove11(:,3)) & zdens < max(square_pointsRemove11(:,3)));

ptGridAvgDownS.Location(xyzdens,:)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% NUVEM RECORTADA
figure
plot3(square_pointsRemove11(:,1),square_pointsRemove11(:,3),-square_pointsRemove11(:,2),'k.')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')

pc1111 = pointCloud(square_pointsRemove11);

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

pcdown111 = pcdownsample(pc1111,"gridAverage",0.001);

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