%%
%Assignment1 by Baylie Meredith
%%
function assign(folder)
clc();
if ~exist('folder','var')
    folder = '.\data\HomeC002\';
end

disp(folder);

% load Depth and RGB images.
tempload = load([folder,'\PSLR_C01_120x160.mat']);
RGB = tempload.CC;
tempload = load([folder,'\PSLR_D01_120x160.mat']);
DEPTH= tempload.CR;

L  = DEPTH.N;
global globvar1;
globvar1=[];
globvar1.flagPause = 0;
globvar1.flagrotate = 1;
globvar1.flagfilter = 1;

%% Create figure, where we will show Depth & RGB
figure(1);
clf();

% Subfigure, for Depth
subplot(2,1,1) ;
depthinr = DEPTH.R(:,:,1);
HandleDepth = imagesc(depthinr);
title('Depth');
colormap gray;
set(gca(),'xdir','reverse'); %flipping axes

% In another subfigure, we show the associated RGB image.
subplot(2,1,2);
HandleColour = image(RGB.C(:,:,:,1));
title('RGB');
set(gca(),'xdir','reverse'); %flipping axes

%% 3D points figure.
figure(2);
clf();

HandleAxes = axes('position',[0.20,0.1,0.75,0.85]); %moves the 3D in the graph
HandlePlot1 = plot3(HandleAxes,0,0,0,'.','markersize',2) ;
HandlePlotr = plot3(HandleAxes,0,0,0,'.r','markersize',2) ;
axis([0,3,-1.5,1.5,-0.4,0.9]);
title('3D');
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Z (m)');
grid on;
rotate3d on ;

%% Some control buttons (you may define extra buttons, for other purposes)

uicontrol('Style','pushbutton','String','Pause/Cont.','Position',[10,1,80,20],'Callback',{@MyCallBackA,1});
uicontrol('Style','pushbutton','String','ETC','Position',[90,1,80,20],'Callback',{@MyCallBackA,2});
uicontrol('Style','slider','Position',[10,50,40,150],'Callback',{@MyCallBackB,1});
uicontrol('Style','checkbox','Position',[20,250,30,50],'Callback',{@MyCallBackRotate});
uicontrol('Style','checkbox','Position',[10,250,30,50],'Callback',{@MyCallBackFilter});

iradius = input('Inner radius =');
oradius = input('Outer radius =');
%%
%theta = 0:0.01:2*pi
%r = oradius
%x = r*cos(theta)
%y = r*sin(theta)
%z = [0]
%plot3(x,y,z);
%r = iradius
%x = r*cos(theta)
%y = r*sin(theta)
%plot3(x,y,z);

i=0;
while 1 %neverending loop like 2121
    while (globvar1.flagPause) %while this flagpause is true
        pause(0.3); %stops and checks every 0.3 seconds whether flagpause has been turned off
    end

    i=i+1;
    if i > L
        break;
    end

    depthinr = DEPTH.R(:,:,i);
    set(HandleColour,'cdata',RGB.C(:,:,:,i));  % show RGB image
    set(HandleDepth,'cdata',depthinr);

    % obtain 3D points, for those pixels which are not faulty.
    Goodpixel = find(depthinr>0);
    [row,col] = find(depthinr>0);
    %changes the numbers on the 3D plot
    if (globvar1.flagrotate == 1)
        [xx,yy,zz] = ConvertSelectedDepthsTo3DPoints(single(depthinr)*0.001,Goodpixel,row,col);
        [xx,zz] = Rotate2D(xx,zz,10);
        zz = zz + 0.2;
    end

    % Show the 3D points but not below -5 or above 100cm
    cutindexes = (zz < 1) & (zz < 1);
    [xx,yy,zz] = CutOff(xx,yy,zz,cutindexes);

    if(globvar1.flagfilter == 1)
        radius = (xx.^2 +yy.^2).^(1/2);
        redpoints = ((radius < oradius ) & (radius > iradius) & (zz > 0.15))
        %gets a logically vector of the indices that are red hence ~red will be
        %blue points
        xred = xx(redpoints)
        xblue = xx(~redpoints)
        yred = yy(redpoints)
        yblue = yy(~redpoints)
        zred = zz(redpoints)
        zblue = zz(~redpoints);
         hold on;
        set(HandlePlot1,'xdata',xblue,'ydata',yblue,'zdata',zblue);

        set(HandlePlotr,'xdata',xred,'ydata',yred,'zdata',zred);
        hold off;
    %else
     %   set(HandlePlot1,'xdata',xx,'ydata',yy,'zdata',zz);
    end
     pause(0.1);
  end
end
% ---------------------------------------
% Callback function. I defined it, and associated it to certain GUI button,
function MyCallBackA(~,~,x)
    global globvar1;

    if (x == 1)
       globvar1.flagPause = ~globvar1.flagPause; %Switch ON->OFF->ON -> and so on.
       disp(x);disp(globvar1.flagPause);
       return;
    end
    if (x==2)
        disp('you pressed ETC!');
        return;
    end
    return;
end

% ...............................................
% I associated the following function, as a callback function for one slider control.
% Each time a new value is set, in the slider, our function is called
function MyCallBackB(a,~,~)
    global globvar1
    globvar1 = get(a,'value');     % the property "value" is the current
                            % value of the slider (position of the selector)
    % You may use it to set the value of certain relevant variable,
    % in your program.
    % Here, I just print its value, for testing purposes.
      disp(v) ;

    % BTW: the object ("a"), has many other properties; you may inspect them.
    return;
end
% ---------------------------------------
% I associated this function, as a callback function for one CheckBox.
% Each time the state of the checkbx is modified, our function is called
function MyCallBackRotate(a,~,~)
    global globvar1
    globvar1.flagrotate = get(a,'value');
    return;
end
function MyCallBackFilter(a,~,~)
    global globvar1
    globvar1.flagFilter = get(a,'value');
    return;
end
% ---------------------------------------
function [xx,yy,zz] = ConvertSelectedDepthsTo3DPoints(depthfunc, Goodpixels, r,c)
    xx = depthfunc(Goodpixels);
    yy = depthfunc(Goodpixels).*(c-80)*4/594;
    zz = -depthfunc(Goodpixels).*(r-60)*4/592;
end
% ---------------------------------------
function [xx,zz] = Rotate2D(xcoord,zcoord,angle)
    angle = angle*pi/180;
    xx = xcoord*cos(angle)+ zcoord*sin(angle);
    zz = xcoord*-sin(angle)+ zcoord*cos(angle);
end
%---------------------------------------
function [xx,yy,zz] = CutOff(xx,yy,zz,cutindices)
    zz = zz(cutindices);
    yy = yy(cutindices);
    xx = xx(cutindices);
end