close all
clear all
clc

%% CAMARAS
% LIGAR CAMARA RGB
RGBDisp = videoinput('kinect',1,'RGB_640x480');

% LIGAR CAMARA DEPTH
DEPTHDisp = videoinput('kinect',2,'Depth_640x480');
preview(DEPTHDisp)
pause(16)

% RETIRAR FRAMES DE AMBAS AS CAMARAS 
RGBFrame = getsnapshot(RGBDisp);
DEPTHFrame = getsnapshot(DEPTHDisp);

% AMBOS FRAMES TIVERAM DE SER RODADOS PARA FICAREM NORMAIS
% O FRAME PARA A NUVEM DE PONTOS NÃO PODE SER RODADO
RGBFrameFlipped = flip(RGBFrame,2);
DEPTHFrameFlipped = flip(DEPTHFrame,2);

%% NUVEM DE PONTOS COMPLETA
% EXTRAIR A NUVEM DE PONTOS
ptCloudOriginal = pcfromkinect(DEPTHDisp, DEPTHFrame, RGBFrame);

% MOSTRAR NUVEM DE PONTOS
% figure
% pcshow(ptCloudOriginal,"VerticalAxis","Y",VerticalAxisDir="Down");
% title('Original Point Cloud')
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')

%% RODAR A NUVEM DE PONTOS PARA FICAR PARALELA COM EIXO DOS XX
% ANGULO DE ROTAÇAO
theta = -0.055;

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

% % MOSTRAR NUVEM DE PONTOS
% figure
% pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down");
% title('Rotated Point Cloud')
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')

%% POINT CLOUD PARALLEL TO X AXIS

% SET THE MAX POINT TO PLABE DISTANCE FOR PLANE FITTING
maxDistance = 0.02;     % (m)

% SET THE NORMAL VECTOR OF THE PLANE
referenceVector = [0,0,1];

% SET THE MAX ANGULAR DISTANCE TO 5 DEGREES
maxAngularDistance = 5;

% DETECT THE FIRST PLANE AND EXTRACT IT FROM THE POINT CLOUD
[model1,inlierIndices,outlierIndices] = pcfitplane(ptCloudRot,maxDistance,referenceVector,maxAngularDistance);
plane1 = select(ptCloudRot,inlierIndices);
remainPtCloud = select(ptCloudRot,outlierIndices);

% SET THE REGION OF INTEREST TO CONSTRAIN THE SEARCH FOR THE SECOND PLANE
roi = [-inf,inf;0.4,inf;-inf,inf];
sampleIndices = findPointsInROI(remainPtCloud,roi);

% DETECT THE FLOOR PLANE AND EXTRACT IT FROM THE REMAINING POINT CLOUD
[model2,inlierIndices,outlierIndices] = pcfitplane(remainPtCloud,maxDistance,SampleIndices=sampleIndices);
plane2 = select(remainPtCloud,inlierIndices);
remainPtCloud = select(remainPtCloud,outlierIndices);

% % MOSTRAR O 1º PLANO AKA PLANO DO FUNDO
% figure
% pcshow(plane1,"VerticalAxis","Y",VerticalAxisDir="Down")
% title('first plane')
% 
% % MOSTRAR O 2º PLANO AKA PLANO DO CHAO
% figure
% pcshow(plane2,"VerticalAxis","Y",VerticalAxisDir="Down")
% title('2nd plane')
% 
% % MOSTRAR A RESTANTE NUVEM DE PONTOS SEM OS PLANOS ANTERIORES
% figure
% pcshow(remainPtCloud,"VerticalAxis","Y",VerticalAxisDir="Down")
% title('remain ptcloud')

% FAZER DENOISE DA NUVEM DE PONTOS PARA LIMPAR PERTURBAÇÕES
pclimpa = pcdenoise(remainPtCloud,'NumNeighbors',10);
figure
pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
title('point cloud denoised')






%% TARGET AREA

stepSize = 100;

indices1 = 1:stepSize:ptCloudRot.Count;
tempPtCloud = select(ptCloudRot,indices1);

[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);

worldPoints = tempPtCloud.Location;

[Y,X] = ind2sub([size(ptCloudRot.Location,1),size(ptCloudRot.Location,2)],indices1);
imagePoints = [X(validIndices)' Y(validIndices)'];

camMatrix = estimateCameraMatrix(imagePoints,worldPoints);

roi1 = [-0.5 0.5 0 0.4 1.2 1.7];

indices1 = findPointsInROI(ptCloudRot,roi1,camMatrix);
% [rowY,colY] = ind2sub([480 640],indices1)
ptcloudB = select(ptCloudRot,indices1);

figure
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
pcshow(ptcloudB.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")
% hold off

xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')


hold on

% c=[1 0 0 0 ];
p1 = [-0.5 0.4 1.7]';
p2 = [-0.5 0.4 1.2]';
p3 = [0.5 0.4 1.2]';
p4 = [0.5 0.4 1.7]';
p5 = [-0.5 0 1.7]';
p6 = [-0.5 0 1.2]';
p7 = [0.5 0 1.2]';
p8 = [0.5 0 1.7]';
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];


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


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











% SET THE MAX POINT TO PLABE DISTANCE FOR PLANE FITTING
maxDistance1 = 0.02;     % (m)

% SET THE NORMAL VECTOR OF THE PLANE
referenceVector1 = [0,0,1];

% SET THE MAX ANGULAR DISTANCE TO 5 DEGREES
maxAngularDistance1 = 5;

% DETECT THE FIRST PLANE AND EXTRACT IT FROM THE POINT CLOUD
[model3,inlierIndices1,outlierIndices1] = pcfitplane(ptcloudB,maxDistance1,referenceVector1,maxAngularDistance1);
plane3 = select(ptcloudB,inlierIndices1);
remainPtCloud1 = select(ptcloudB,outlierIndices1);

% SET THE REGION OF INTEREST TO CONSTRAIN THE SEARCH FOR THE SECOND PLANE
roi2 = [-inf,inf;0,inf;-inf,inf];
sampleIndices1 = findPointsInROI(remainPtCloud1,roi2);

% DETECT THE FLOOR PLANE AND EXTRACT IT FROM THE REMAINING POINT CLOUD
[model4,inlierIndices1,outlierIndices1] = pcfitplane(remainPtCloud1,maxDistance1,SampleIndices=sampleIndices1);
plane4 = select(remainPtCloud1,inlierIndices1);
remainPtCloud1 = select(remainPtCloud1,outlierIndices1);


figure
pcshow(remainPtCloud1,"VerticalAxis","Y",VerticalAxisDir="Down")
title('remain ptcloud')
figure
pcshow(plane3,"VerticalAxis","Y",VerticalAxisDir="Down")
title('1st plane')
figure
pcshow(plane4,"VerticalAxis","Y",VerticalAxisDir="Down")
title('2nd plane')



























% ptCloudRotX = ptCloudRot.Location(:,:,1);
% ptCloudRotY = ptCloudRot.Location(:,:,2);
% ptCloudRotZ = ptCloudRot.Location(:,:,3);
% 
% xLimPtCRot = find(ptCloudRotX>-0.5 & ptCloudRotX<0.5);
% yLimPtCRot = find(ptCloudRotY>-0.4 & ptCloudRotY<0.4);
% zLimPtCRot = find(ptCloudRotZ>1.15 & ptCloudRotZ<2.15);
% 
% novanuvem = [ptCloudRotX(xLimPtCRot),ptCloudRotY(yLimPtCRot),ptCloudRotZ(zLimPtCRot)];
% scatter3(ptCloudRotX(xLimPtCRot),ptCloudRotY(yLimPtCRot),ptCloudRotZ(zLimPtCRot))
% pcshow()




% %% ROI 
% figure
% imshow(RGBFrameFlipped)
% 
% roi = drawassisted('StripeColor','y');
% 
% roi = images.roi.AssistedFreehand('Color','c','StripeColor','r');
% 
% roi.Parent
% 
% draw(roi)















% %% ROTAÇAO 1
% ylimmin = -1.3;
% ylimax = 0.8;
% 
% 
% a = ptCloudOriginal.Location(:,:,2)>ylimmin;
% b = ptCloudOriginal.Location(:,:,3)<ylimax;
% % yIdx = ptCloudOriginal.Location(:,:,2)>ylimmin & ptCloudOriginal.Location(:,:,3)<ylimax; 
% ptCloudSegmented = ptCloudOriginal.Location(); 
% 
% figure
% pcshow(ptCloudSegmented,"VerticalAxis","Y",VerticalAxisDir="Down");
