clear all
close all
clc

% 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")
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

%% NORMAIS DOS PONTOS DA NUVEM DE PONTOS DO OBJETO
% NORMAIS DOS PONTOS DA NUVEM DE PONTOS DO OBJETO

% knormals = 10000;
for i=100:100:1000
%     i=100:100:1000
    normals = pcnormals(pclimpa,i);
    % normals = pcnormals(pclimpa,knormals);

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

    % % MOSTRAR OS PONTOS DA NUVEM DO OBJETO
    % figure
    % pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
    % hold on

    % VETORES UNITARIOS
    u = normals(:,1);
    v = normals(:,2);
    w = normals(:,3);

    % % MOSTRAR AS NORMAIS DE CADA PONTO DA NUVEM DO OBJETO
    % q = quiver3(xnuvem,ynuvem,znuvem,u,v,w);
    % title('Original quiver k = 5')

    % 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")
    % title(['Adjusted Normals of Point Cloud k =',num2str(knormals)])
    title(['Adjusted Normals of Point Cloud k =',num2str(i)])
    hold on
    q = quiver3(xnuvem, ynuvem, znuvem, u, v, w);
    hold off
end