% reading and visualizaing data from point clouds

directory= uigetdir('C:\Users\margo_kat\Documents\GitHub\cloud\cloud2');
% get the right path for finding files 
% with filename cloud and .txt extension
sdir=strcat(directory,'\*pdflow_cloud*.txt');
% get the files with DATA as filename & .csv as extension
% in the choosen folder
files=dir(sdir);

sdir=strcat(directory,'\*pdflow_results*.txt');
files_flow=dir(sdir);


for i=1:size(files,1)
    name=files(i,1);
    XYZ_cloud=makeFlowIm(name);
    name.name
    name=files_flow(i,1);
    Point_Flow=makeFlowIm(name);
    name.name
   
% get cursor coordinate
%% Data cursor


if (i==1)
    
    fig=figure();
    subplot(1,2,1);
   
    scatter3(reshape(XYZ_cloud(:,:,1), 240*320,1),reshape(XYZ_cloud(:,:,2),240*320,1),reshape(XYZ_cloud(:,:,3),240*320,1), 2);
    hold on
    pause()
    
    Points_track=zeros(5,10);
    for j=1:10
        datacursormode on
        dcmObj = datacursormode(fig);
        set(dcmObj,'SnapToDataVertex','off','Enable','on')
        loop = 1;
        while loop == 1
            waitforbuttonpress;
            point1 = getCursorInfo(dcmObj);
            z = point1.Position(1);
            x = -point1.Position(2);
            y = point1.Position(3);
            loop=0;
    % Write exit option
        end
        datacursormode off
% display data in 2d coordinate 480X640 
% Z=depth.at<float>(v,u) , X=depth.at<float>(v,u)*(u - cx_d)  / fx_d ,  Y=depth.at<float>(v,u)*(cy_d - v) / fy_d);
%  240x320
%     u=floor((x*5.9421434211923247e+02)/z +(3.3930780975300314e+02))/2
%     v=floor((y*5.9104053696870778e+02)/z+ (2.4273913761751615e+02))/2

 u=round(((3.3930780975300314e+02/2)-x*5.9421434211923247e+02/z))
 v=round(((2.4273913761751615e+02/2)-y*5.9104053696870778e+02/z))
 if u>240
     u=240;
 end
     if v>320
         v=320;
     end
    
     RGB = insertMarker(Point_Flow.*255,[u v],'x','color','red','size',10);
     subplot(1,2,2)
     imshow(RGB);
    Points_track(:,j)=[u,v, z, y, x];

    end
'Points selected'
hold off;
close all;
end
% else %part to find tracking correposndences
%    Points_track=FindNNPoints(deplaced_coord, XYZ_cloud);
% end
% part to find correspondance
deplaced_coord=Calculate_New_Coord(Points_track, Point_Flow);
%scatter3(deplaced_coord(3,:), deplaced_coord(4,:), deplaced_coord(5,:),'r', 'fill');


pos   = [Points_track(1,1), Points_track(2,1);Points_track(1,2), Points_track(2,2); Points_track(1,3), Points_track(2,3);Points_track(1,4), Points_track(2,4); Points_track(1,5), Points_track(2,5); Points_track(1,6), Points_track(2,6);...
Points_track(1,7), Points_track(2,7); Points_track(1,8), Points_track(2,8); Points_track(1,9), Points_track(2,9); Points_track(1,10), Points_track(2,10)];
color = {'red','white','green','magenta', 'blue', 'yellow', 'cyan', 'red', 'white', 'green'};
FLowIm=zeros(240,320,3);
FlowIm(:,:, 1)=Point_Flow(:,:,3).*100;
RGB =insertMarker(FlowIm, pos,'x','color',color,'size',4);
figure
imshow(RGB);
pause(1);



end