clc; clear ; close all

% CARREGAR WORKSPACE COM A NUVEM DO AMBIENTE CAPTURADA PELA KINECT
load("workspace_cloud.mat")

% NUVEM DE PONTOS DO OBJETO
figure("Name",'Object Cloud Viewer','NumberTitle','off');
pcshow(pclimpa,"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)')

%% NONUNIFORMGRIDSAMPLE
maxNumPoints = 50;

% CLOUD DOWNSAMPLING USING NONUNIFORMGRIDSAMPLE METHOD
ptNonUnifDownS = pcdownsample(pclimpa,'nonuniformGridSample',maxNumPoints);


figure
pcshow(ptNonUnifDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
% title(['Point Cloud NonUniformGridSample DownSampling with maxNumPoints = ',num2str(maxNumPoints)])
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% NONUNIFORMGRIDSAMPLE NORMALS
uNonUnifDownS = ptNonUnifDownS.Normal(:,1);
vNonUnifDownS = ptNonUnifDownS.Normal(:,2);
wNonUnifDownS = ptNonUnifDownS.Normal(:,3);

figure
% subplot(2,1,1)
pcshow(ptNonUnifDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
hold on
quiver3(ptNonUnifDownS.Location(:,1),ptNonUnifDownS.Location(:,2),ptNonUnifDownS.Location(:,3), uNonUnifDownS, vNonUnifDownS, wNonUnifDownS);
hold off
title('NonUniformGridSample Point Cloud Normals')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% NONUNIFORMGRIDSAMPLE LOCATION POINTS
xNonUnifDownS = ptNonUnifDownS.Location(:,1);
yNonUnifDownS = ptNonUnifDownS.Location(:,2);
zNonUnifDownS = ptNonUnifDownS.Location(:,3);

% ALINHAR AS NORMAS COM O CENTRO DO REFERENCIAL AKA CENTRO DA KINECT
sensorCenter = [0,0,0];
for k = 1 : numel(xNonUnifDownS)
    p1 = sensorCenter - [xNonUnifDownS(k),yNonUnifDownS(k),zNonUnifDownS(k)];
    p2 = [uNonUnifDownS(k),vNonUnifDownS(k),wNonUnifDownS(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
        uNonUnifDownS(k) = -uNonUnifDownS(k);
        vNonUnifDownS(k) = -vNonUnifDownS(k);
        wNonUnifDownS(k) = -wNonUnifDownS(k);
    end
end

figure
pcshow(ptNonUnifDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
hold on
quiver3(xNonUnifDownS,yNonUnifDownS,zNonUnifDownS, uNonUnifDownS, vNonUnifDownS, wNonUnifDownS);
hold off
title('NonUniformGridSample Point Cloud Aligned Normals')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% CALCULAR NORMAIS
normalsNonUnifDownS = pcnormals(ptNonUnifDownS,maxNumPoints);

% VETORES UNITARIOS
uNonUnifDownS1 = normalsNonUnifDownS(:,1);
vNonUnifDownS1 = normalsNonUnifDownS(:,2);
wNonUnifDownS1 = normalsNonUnifDownS(:,3);

% MOSTRAR NORMAS ALINHADAS
figure
pcshow(ptNonUnifDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
hold on
quiver3(xNonUnifDownS, yNonUnifDownS, zNonUnifDownS, uNonUnifDownS1, vNonUnifDownS1, wNonUnifDownS1);
hold off
title('NonUniformGridSample 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(xNonUnifDownS)
    p1 = sensorCenter - [xNonUnifDownS(k),yNonUnifDownS(k),zNonUnifDownS(k)];
    p2 = [uNonUnifDownS1(k),vNonUnifDownS1(k),wNonUnifDownS1(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
        uNonUnifDownS1(k) = -uNonUnifDownS1(k);
        vNonUnifDownS1(k) = -vNonUnifDownS1(k);
        wNonUnifDownS1(k) = -wNonUnifDownS1(k);
    end
end

% MOSTRAR NORMAS ALINHADAS
figure
pcshow(ptNonUnifDownS,"VerticalAxis","Y",VerticalAxisDir="Down",BackgroundColor=[1 1 1])
hold on
quiver3(xNonUnifDownS, yNonUnifDownS, zNonUnifDownS, uNonUnifDownS1, vNonUnifDownS1, wNonUnifDownS1);
hold off
title('NonUniformGridSample Point Cloud aligned pcNormals Normals')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')